#include "motionform.h"
#include "ui_motionform.h"
#include "MacroDefine/GeneralDefine.h"
#include "mccontrollerinterpreter.h"
#include "mccontrollerthread.h"
#include <qmessagebox.h>
#include "balljoystick.h"
#include <QFileDialog>

MotionForm::MotionForm(McControllerInterpreter* mcInterpreterIn,int robotIdIn,bool isAutoSaveWaveIn
                       ,QWidget *parent) :
    QWidget(parent),
    ui(new Ui::MotionForm)
{
    ui->setupUi(this);
    robotId=robotIdIn;

//    setWindowFlags(Qt::CustomizeWindowHint);
    QString tmpStr="MotionForm-robotId ";
    tmpStr+=QString::number(robotIdIn);
    setWindowTitle(tmpStr);

    leftSpeedCommand=0;
    rightSpeedCommand=0;
    debugFlag=0;

    ui->horizontalSlider->setRange(0,100);
    ui->horizontalSlider_2->setRange(0,100);

    ui->horizontalSlider->setValue(30);
    ui->horizontalSlider_2->setValue(30);

    mcInterpreter=mcInterpreterIn;
    timer = new QTimer();
    connect(timer,SIGNAL(timeout()),this,SLOT(timeroutslot()));//timeoutslot()为自定义槽
    if(NULL!=mcInterpreter)
    {
        PingPongBuffer *buffer=mcInterpreter->getPingPongBuffer();
        motorView= new MotorStatusView(buffer, mcInterpreter->jointNum,robotIdIn,isAutoSaveWaveIn);

    }
    else
    {
        ui->label_16->setText("mcInterpreter inital failed!");
    }

    ballJoyStick=new BallJoyStick(400,this);

    ////initial
    model_matchInfo = new QStandardItemModel();
    QStringList labels0 = QObject::trUtf8("变量,value,-").simplified().split(",");
    model_matchInfo->setHorizontalHeaderLabels(labels0);
    ui->tableView_matchDebugInfo->setColumnWidth(1,120);

    model_algroCallTimeInfo=new QStandardItemModel();
    model_timeConsumeInfo= new QStandardItemModel();
    model_vmarksInfoInfo= new QStandardItemModel();
    model_vmarksDebugInfo= new QStandardItemModel();
    model_qrcodeDebugInfo= new QStandardItemModel();
    model_encoderReceiverInfo= new QStandardItemModel();
    model_beltTrackDebugInfo= new QStandardItemModel();
    model_reflectDebugInfo= new QStandardItemModel();
    model_amclDebugInfo= new QStandardItemModel();
    model_regulatorDebugInfo= new QStandardItemModel();
    model_imuData= new QStandardItemModel();
    model_emcTpInfo=new QStandardItemModel();
    model_simpleTp=new QStandardItemModel();
    //
    ui->tableView_emcTpDebugInfo->setColumnWidth(0,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(1,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(2,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(3,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(4,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(5,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(6,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(7,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(8,20);
    ui->tableView_emcTpDebugInfo->setColumnWidth(9,20);

}

MotionForm::~MotionForm()
{
    delete ui;
}

int MotionForm::showThisDialog()
{
    setGeometry(100,300,1200,800);
    show();
    showWaveForm();
}

int MotionForm::clearMessage()
{
    return motorView->clearMessage();
}

int MotionForm::startMotorWave()
{
    motorView->startWave();
    return 1;
}

int MotionForm::stopMotorWave()
{
    motorView->stopWave_delay();
    return 1;
}


void MotionForm::timeroutslot()
{
    if(1==debugFlag)
     {
         qDebug()<<"MotionForm::timeroutslot() In";
     }

    showPositionType();

    //read pvct feed back
    showJointFeedback();

    showJointCommand();

    directVelocityCommand();

    directPositionCommand();

    //read odometer
    showOdometerInfo();



    displayMaganeticSensorInfo();
    displayRfidSensorInfo();


    //joy stick control
    if(ui->checkBox_isVehicleJognew->isChecked())
    {
        vehicleJog();
    }

    //show vehicle vel command
    displayVehicleVelCommand();


    //show Vehicle Positon
    displayVehiclePosition();


    //show maganetic status
    displayMaganeticTrackInfo();

    showRosMovingStatus();

    displayVirtualLineInfo();

    //show match debug info
    PclMatchDebugInfo matchInfo;
    mcInterpreter->getMatchDebugInfo(matchInfo);
    showDebugInfo_matchInfo(matchInfo);
    mcInterpreter->updatePclIcpDebugWindow();
    on_pushButton_drawVisionMatchWindow_clicked();

    std::vector<EncoderReceiverData> encoderInfo;
    mcInterpreter->getEncoderReceiverData(encoderInfo);
    if(encoderInfo.size()>0)
    {
        int tmpIndex=ui->comboBox_currentEncoderReceiver->currentIndex();
        if(tmpIndex<encoderInfo.size())
        {
            displayEncoderReceiver(encoderInfo[tmpIndex]);
        }
        else
        {
            qDebug()<<"no such encoder device!!!";
        }
    }


    showRosApiTimeConsume();
    showAlgroCallTime();

    displayVmarkDebugInfo();
    displayReflectDebugInfo();
    displayAmclDebugInfo();

    updateVmarkPositionInworld();

    displayCu1DebugInfo();

    displayQrcodeDebugInfo();

    displayLaserDriverStatus();

    if(ui->checkBox_getTrackInfo_belt->isChecked())
    {
        displayBeltTrackDebugInfo();
    }

    if(ui->checkBox_isUpdateRegulatorDebugInfo->isChecked())
    {
        displayRegulatorDebugInfo();
    }



    if(ui->checkBox_getTrackInfo_vision->isChecked())
    {
        displayVisionTrackDebugInfo();

    }

    ImuDataInfo imuData;
    mcInterpreter->getImuDeviceData(imuData);
    displayImuData(imuData);

    if(ui->checkBox_isUpdateEmcTpInfo->isChecked())
    {
        EmcDebugInfo emcTpInfo;
        mcInterpreter->getEmcDebugInfo(emcTpInfo);
        displayEmcDebugInfo(emcTpInfo);
    }

    ui->comboBox_tpType->setCurrentIndex(mcInterpreter->isInCarvingMode());

    if(ui->checkBox_isUpdateSimpleTp->isChecked())
    {
        SimpleTpInfo simpleTpInfo;
        mcInterpreter->getAxisSimpleTpInfo(ui->lineEdit_axisId->text().toInt(), simpleTpInfo);
        displaySimpleTpInfo(simpleTpInfo);
    }



    if(1==debugFlag)
    {
        qDebug()<<"MotionForm::timeroutslot() Out";
    }
}

int MotionForm::displayMaganeticSensorInfo()
{
    //sensor display
    int aliasIn=ui->comboBox_aliasIn->currentText().toInt();
    QVector<bool> sensorRawValueOut ;
    int returnKey=mcInterpreter->getMaganeticSensorRawValue(aliasIn,sensorRawValueOut);

    if(1!=returnKey)
    {
        return -1;
    }

    ui->label_8->setText("update di");

    if(true==sensorRawValueOut[0])
    {
        ui->checkBox_3->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_3->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[1])
    {
        ui->checkBox_4->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_4->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[2])
    {
        ui->checkBox_5->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_5->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[3])
    {
        ui->checkBox_6->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_6->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[4])
    {
        ui->checkBox_7->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_7->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[5])
    {
        ui->checkBox_8->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_8->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[6])
    {
        ui->checkBox_9->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_9->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[7])
    {
        ui->checkBox_10->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_10->setCheckState(Qt::Unchecked);
    }

    if(true==sensorRawValueOut[8])
    {
        ui->checkBox_20->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_20->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[9])
    {
        ui->checkBox_21->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_21->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[10])
    {
        ui->checkBox_22->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_22->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[11])
    {
        ui->checkBox_23->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_23->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[12])
    {
        ui->checkBox_24->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_24->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[13])
    {
        ui->checkBox_25->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_25->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[14])
    {
        ui->checkBox_26->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_26->setCheckState(Qt::Unchecked);
    }
    if(true==sensorRawValueOut[15])
    {
        ui->checkBox_27->setCheckState(Qt::Checked);
    }
    else
    {
       ui->checkBox_27->setCheckState(Qt::Unchecked);
    }
    return 1;
}

int MotionForm::displayRfidSensorInfo()
{
    //sensor display
    int aliasIn=ui->comboBox_rfid_aliasIn->currentText().toInt();
//    QVector<unsigned char> cardValueOut;
    bool isCardDetected;
    long long cardValueLong;
    int returnKey=mcInterpreter->getCurrentTagData(aliasIn,cardValueLong,isCardDetected);

    if(1!=returnKey)
    {
        return -1;
    }

    ui->lineEdit_RFID_data_1->setText(QString::number(cardValueLong));
//    ui->lineEdit_RFID_data_2->setText(QString::number(cardValueOut[1]));
//    ui->lineEdit_RFID_data_3->setText(QString::number(cardValueOut[2]));
//    ui->lineEdit_RFID_data_4->setText(QString::number(cardValueOut[3]));
//    ui->lineEdit_RFID_data_5->setText(QString::number(cardValueOut[4]));
//    ui->lineEdit_RFID_data_6->setText(QString::number(cardValueOut[5]));
//    ui->lineEdit_RFID_data_7->setText(QString::number(cardValueOut[6]));
//    ui->lineEdit_RFID_data_8->setText(QString::number(cardValueOut[7]));

    if(true==isCardDetected)
    {
        ui->lineEdit_RFID_isExist->setText("card detected");
    }
    else
    {
        ui->lineEdit_RFID_isExist->setText("none card detected");
    }

    return 1;
}

int MotionForm::displayMaganeticTrackInfo()
{
    E_TRACK_STATUS tmpStatus;
    mcInterpreter->getTrackStatus(tmpStatus);
    switch(tmpStatus)
    {
    case E_TRACK_STATUS_STOPPED:
    {
        ui->label_maganeticTrackStatus->setText("stopped");
        break;
    }
    case E_TRACK_STATUS_RUNNING:
    {
        ui->label_maganeticTrackStatus->setText("running");
        break;
    }
    case E_TRACK_STATUS_GOAL_REACHED:
    {
        ui->label_maganeticTrackStatus->setText("reached");
        break;
    }
    case E_TRACK_STATUS_ERROR:
    {
        ui->label_maganeticTrackStatus->setText("error");
        break;
    }
    }
}

int MotionForm::displayVirtualLineInfo()
{
    VirtualLineDebugInfo infoOut;
    mcInterpreter->getVirtuleLineDebugInfo(infoOut);
    ui->lineEdit_deltaAngle->setText(QString::number(infoOut.deltaAngle,'f',3));
    ui->lineEdit_deltaDistance->setText(QString::number(infoOut.deltaDistance,'f',3));
    ui->lineEdit_deltaDistance_y->setText(QString::number(infoOut.deltaDistance_y,'f',3));
    ui->lineEdit_absoluteAngle->setText(QString::number(infoOut.absoluteAngle,'f',3));
    ui->lineEdit_absoluteDistance->setText(QString::number(infoOut.absoluteDistance,'f',3));

    double anglePidOutput,xPidOutput,yPidOutput;
    mcInterpreter->getTrackPidOutput(anglePidOutput,xPidOutput,yPidOutput);
    ui->lineEdit_pathPidOutput->setText(QString::number(anglePidOutput,'f',3));
    ui->lineEdit_velocityPidOut->setText(QString::number(xPidOutput,'f',3));
    ui->lineEdit_velocityYPidOut->setText(QString::number(yPidOutput,'f',3));

    double currentLineOffsetOut, currentAngleOffsetOut;
    mcInterpreter->getVehicleFollowOffset(currentLineOffsetOut,currentAngleOffsetOut);
    ui->lineEdit_virtualLineFollowOffset_angle->setText(QString::number(currentAngleOffsetOut,'f',3));
    ui->lineEdit_virtualLineFollowOffset_distance->setText(QString::number(currentLineOffsetOut,'f',3));
    return 1;
}

int MotionForm::directVelocityCommand()
{
    // 直接发电机速度vel control
        if(Qt::Checked==ui->checkBox->checkState())
        {
            QVector<double> velocity;
            velocity.resize(jointNumer);
            if(1<=jointNumer)
            {
                 velocity[0]=ui->lineEdit_velCommandJ1->text().toDouble();
            }
            if(2<=jointNumer)
            {
                 velocity[1]=ui->lineEdit_velCommandJ2->text().toDouble();
            }
            if(3<=jointNumer)
            {
                 velocity[2]=ui->lineEdit_velCommandJ3->text().toDouble();
            }
            if(4<=jointNumer)
            {
                 velocity[3]=ui->lineEdit_velCommandJ4->text().toDouble();
            }
            if(5<=jointNumer)
            {
                 velocity[4]=ui->lineEdit_velCommandJ5->text().toDouble();
            }
            if(6<=jointNumer)
            {
                 velocity[5]=ui->lineEdit_velCommandJ6->text().toDouble();
            }
            if(7<=jointNumer)
            {
                 velocity[6]=ui->lineEdit_velCommandJ7->text().toDouble();
            }
            if(8<=jointNumer)
            {
                 velocity[7]=ui->lineEdit_velCommandJ8->text().toDouble();
            }

            mcInterpreter->moveOneStepByVelocity(velocity);
        }



        return 1;
}

int MotionForm::directPositionCommand()
{

    // 直接发电机速度vel control
    if(Qt::Checked==ui->checkBox->checkState())
    {
        if(0==ui->comboBox->currentIndex())
        {
            if(1<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ1->text().toDouble(),0,0);
            }
            if(2<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ2->text().toDouble(),0,1);
            }
            if(3<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ3->text().toDouble(),0,2);
            }
            if(4<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ4->text().toDouble(),0,3);
            }
            if(5<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ5->text().toDouble(),0,4);
            }
            if(6<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ6->text().toDouble(),0,5);
            }
            if(7<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ7->text().toDouble(),0,6);
            }
            if(8<=jointNumer)
            {
                mcInterpreter->moveOneStepByPV(ui->lineEdit_positionCommandJ8->text().toDouble(),0,7);
            }

        }
        else
        {
        }
    }
    return 1;
}

int MotionForm::displayVehiclePosition()
{
    E_VEHICLE_POSITION_TYPE typeIn=(E_VEHICLE_POSITION_TYPE)ui->comboBox_positionMethod->currentIndex();
    PositionInfo positionReturn;
    mcInterpreter->getVehiclePosition_specified(positionReturn,typeIn);
    ui->lineEdit_rosPosX->setText(QString::number(positionReturn.position.x,'f',3));
    ui->lineEdit_rosPosY->setText(QString::number(positionReturn.position.y,'f',3));
    ui->lineEdit_rosPosR->setText(QString::number(180.0*positionReturn.position.rotate/M_PI,'f',1));
    float weight_out;
    double mapDistance;
    std::vector<float> pos_cov_out;
    mcInterpreter->getAmclMatchScore(mapDistance,weight_out,pos_cov_out);
    QString tmpStr;
    for(int i=0;i<pos_cov_out.size();i++)
    {
        tmpStr+=QString::number(pos_cov_out[i],10,2);
        tmpStr+=", ";
    }

    ui->lineEdit_amcl_match_cov->setText(tmpStr);
    ui->lineEdit_weightScore->setText(QString::number(weight_out,10,3));
    ui->lineEdit_rosAmclMapDistance->setText(QString::number(mapDistance,10,3));
    return 1;
}

int MotionForm::displayVmarksInfoList()
{
    QMap<int, VMarkInfo> vmarkInfoOut;
    mcInterpreter->getVmarkInfoList(vmarkInfoOut);

    model_vmarksInfoInfo->clear();
    //定义item
    QStandardItem* item = 0;
    VMarkInfo tmpVmarkInfo;
    int skipCount=8;
    int i=0;
    foreach(tmpVmarkInfo,vmarkInfoOut)
    {
        item = new QStandardItem("vindex");
        model_vmarksInfoInfo->setItem(i*skipCount+0,0,item);
        item = new QStandardItem(QString::number(tmpVmarkInfo.vindex));
        model_vmarksInfoInfo->setItem(i*skipCount+0,1,item);

        item = new QStandardItem("vmarkType");
        model_vmarksInfoInfo->setItem(i*skipCount+1,0,item);
        item = new QStandardItem(QString::number(tmpVmarkInfo.vmarkType));
        model_vmarksInfoInfo->setItem(i*skipCount+1,1,item);

        item = new QStandardItem("mapName");
        model_vmarksInfoInfo->setItem(i*skipCount+2,0,item);
        item = new QStandardItem(QString::fromStdString(tmpVmarkInfo.mapName));
        model_vmarksInfoInfo->setItem(i*skipCount+2,1,item);

        item = new QStandardItem("comment");
        model_vmarksInfoInfo->setItem(i*skipCount+3,0,item);
        item = new QStandardItem(QString::fromStdString(tmpVmarkInfo.comment));
        model_vmarksInfoInfo->setItem(i*skipCount+3,1,item);

        item = new QStandardItem("pose_x");
        model_vmarksInfoInfo->setItem(i*skipCount+4,0,item);
        item = new QStandardItem(QString::number(tmpVmarkInfo.pose_x,'f',3));
        model_vmarksInfoInfo->setItem(i*skipCount+4,1,item);

        item = new QStandardItem("pose_y");
        model_vmarksInfoInfo->setItem(i*skipCount+5,0,item);
        item = new QStandardItem(QString::number(tmpVmarkInfo.pose_y,'f',3));
        model_vmarksInfoInfo->setItem(i*skipCount+5,1,item);

        item = new QStandardItem("pose_r");
        model_vmarksInfoInfo->setItem(i*skipCount+6,0,item);
        item = new QStandardItem(QString::number(tmpVmarkInfo.pose_r,'f',3));
        model_vmarksInfoInfo->setItem(i*skipCount+6,1,item);

        i++;
    }



    ui->tableView_vmarkInfoList->setModel(model_vmarksInfoInfo);
//    ui->tableView_vmarkInfoList->resizeColumnToContents(0);
//    ui->tableView_vmarkInfoList->resizeColumnToContents(1);
//    ui->tableView_vmarkInfoList->resizeColumnToContents(2);
    ui->tableView_vmarkInfoList->show();
}

int MotionForm::displayVmarkDebugInfo()
{
    VisionMatchDebugInfo infoOut;
    mcInterpreter->getMatchDebugInfo_vmark(infoOut);

    model_vmarksDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;


    item = new QStandardItem("isMatchSucess");
    model_vmarksDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoOut.isMatchSucess));
    model_vmarksDebugInfo->setItem(0,1,item);

    item = new QStandardItem("angleOfLinesOut(degree)");
    model_vmarksDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoOut.angleOfLinesOut*180.0/M_PI,'f',3));
    model_vmarksDebugInfo->setItem(1,1,item);

    item = new QStandardItem("struct_Line1Count");
    model_vmarksDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoOut.struct_Line1Count));
    model_vmarksDebugInfo->setItem(2,1,item);

    item = new QStandardItem("currentVmarkType");
    model_vmarksDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoOut.currentVmarkType));
    model_vmarksDebugInfo->setItem(3,1,item);

    item = new QStandardItem("laserFilterType");
    model_vmarksDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoOut.currentLaserFilterType));
    model_vmarksDebugInfo->setItem(4,1,item);


    item = new QStandardItem("minConsumed(ms)");
    model_vmarksDebugInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoOut.minConsumed/1000.0,'f',3));
    model_vmarksDebugInfo->setItem(5,1,item);

    item = new QStandardItem("maxConsumed");
    model_vmarksDebugInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoOut.maxConsumed/1000.0,'f',3));
    model_vmarksDebugInfo->setItem(6,1,item);

    item = new QStandardItem("currentConsumed");
    model_vmarksDebugInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoOut.currentConsumed/1000.0,'f',3));
    model_vmarksDebugInfo->setItem(7,1,item);


    item = new QStandardItem("struct_Line1Corelation");
    model_vmarksDebugInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoOut.struct_Line1Corelation,'f',3));
    model_vmarksDebugInfo->setItem(8,1,item);


    item = new QStandardItem("struct_Line2Corelation");
    model_vmarksDebugInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoOut.struct_Line2Corelation,'f',3));
    model_vmarksDebugInfo->setItem(9,1,item);

    item = new QStandardItem("detectFailedType");
    model_vmarksDebugInfo->setItem(10,0,item);
    item = new QStandardItem(QString::number(infoOut.detectFailedType));
    model_vmarksDebugInfo->setItem(10,1,item);

    item = new QStandardItem("detectedLineCount");
    model_vmarksDebugInfo->setItem(11,0,item);
    item = new QStandardItem(QString::number(infoOut.detectedLineCount));
    model_vmarksDebugInfo->setItem(11,1,item);

    item = new QStandardItem("detectedVmarkCount");
    model_vmarksDebugInfo->setItem(12,0,item);
    item = new QStandardItem(QString::number(infoOut.detectedVmarkCount));
    model_vmarksDebugInfo->setItem(12,1,item);

    item = new QStandardItem("detectedVmarkResultCount");
    model_vmarksDebugInfo->setItem(13,0,item);
    item = new QStandardItem(QString::number(infoOut.detectedVmarkResultCount));
    model_vmarksDebugInfo->setItem(13,1,item);

    item = new QStandardItem("distanceOfLinesOut");
    model_vmarksDebugInfo->setItem(14,0,item);
    item = new QStandardItem(QString::number(infoOut.distanceOfLinesOut,'f',3));
    model_vmarksDebugInfo->setItem(14,1,item);


    ui->tableView_vmarkDebugInfo->setModel(model_vmarksDebugInfo);
    ui->tableView_vmarkDebugInfo->resizeColumnToContents(0);
    ui->tableView_vmarkDebugInfo->resizeColumnToContents(1);
    ui->tableView_vmarkDebugInfo->resizeColumnToContents(2);
    ui->tableView_vmarkDebugInfo->show();
}

int MotionForm::displayReflectDebugInfo()
{
    ReflectDebugInfo infoOut;
    mcInterpreter->getMatchDebugInfo_reflect(infoOut);

    model_reflectDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;


    item = new QStandardItem("isMatchSucess");
    model_reflectDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoOut.isMatchSucess));
    model_reflectDebugInfo->setItem(0,1,item);

    item = new QStandardItem("x");
    model_reflectDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoOut.pose_x,'f',3));
    model_reflectDebugInfo->setItem(1,1,item);

    item = new QStandardItem("y");
    model_reflectDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoOut.pose_y,'f',3));
    model_reflectDebugInfo->setItem(2,1,item);

    item = new QStandardItem("r");
    model_reflectDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoOut.pose_r*180.0/M_PI,'f',3));
    model_reflectDebugInfo->setItem(3,1,item);



    item = new QStandardItem("initialLaserMarkCount");
    model_reflectDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoOut.initialLaserMarkCount));
    model_reflectDebugInfo->setItem(4,1,item);

    item = new QStandardItem("initialReferenceMarkCount");
    model_reflectDebugInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoOut.initialReferenceMarkCount));
    model_reflectDebugInfo->setItem(5,1,item);

    item = new QStandardItem("currentRefSize");
    model_reflectDebugInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoOut.referenceMarks.size()));
    model_reflectDebugInfo->setItem(6,1,item);


    item = new QStandardItem("minConsumed(ms)");
    model_reflectDebugInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoOut.minConsumed/1000));
    model_reflectDebugInfo->setItem(7,1,item);

    item = new QStandardItem("currentConsumed(ms)");
    model_reflectDebugInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoOut.currentConsumed/1000));
    model_reflectDebugInfo->setItem(8,1,item);

    item = new QStandardItem("maxConsumed(ms)");
    model_reflectDebugInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoOut.maxConsumed/1000));
    model_reflectDebugInfo->setItem(9,1,item);

    item = new QStandardItem("搜索范围.x");
    model_reflectDebugInfo->setItem(10,0,item);
    item = new QStandardItem(QString::number(infoOut.positionError.x,'f',3));
    model_reflectDebugInfo->setItem(10,1,item);

    item = new QStandardItem("搜索范围.y");
    model_reflectDebugInfo->setItem(11,0,item);
    item = new QStandardItem(QString::number(infoOut.positionError.y,'f',3));
    model_reflectDebugInfo->setItem(11,1,item);

    item = new QStandardItem("搜索范围.rotate");
    model_reflectDebugInfo->setItem(12,0,item);
    item = new QStandardItem(QString::number(infoOut.positionError.rotate*180.0/M_PI,'f',3));
    model_reflectDebugInfo->setItem(12,1,item);


    item = new QStandardItem("minumIntensity");
    model_reflectDebugInfo->setItem(13,0,item);
    item = new QStandardItem(QString::number(infoOut.minumIntensity));
    model_reflectDebugInfo->setItem(13,1,item);

    item = new QStandardItem("minLaserRange");
    model_reflectDebugInfo->setItem(14,0,item);
    item = new QStandardItem(QString::number(infoOut.minLaserRange));
    model_reflectDebugInfo->setItem(14,1,item);

    item = new QStandardItem("maxLaserRange");
    model_reflectDebugInfo->setItem(15,0,item);
    item = new QStandardItem(QString::number(infoOut.maxLaserRange));
    model_reflectDebugInfo->setItem(15,1,item);


    ui->tableView_reflectDebugInfo->setModel(model_reflectDebugInfo);
    ui->tableView_reflectDebugInfo->resizeColumnToContents(0);
    ui->tableView_reflectDebugInfo->resizeColumnToContents(1);
    ui->tableView_reflectDebugInfo->resizeColumnToContents(2);
    ui->tableView_reflectDebugInfo->show();
}

int MotionForm::displayAmclDebugInfo()
{
    AmclDebugInfo infoOut;
    mcInterpreter->getAmclDebugInfo( infoOut);

    model_amclDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;


    item = new QStandardItem("isCorrectSucess");
    model_amclDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoOut.isCorrectSucess));
    model_amclDebugInfo->setItem(0,1,item);

    item = new QStandardItem("offset_x(m)");
    model_amclDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoOut.correctPositionOffset.x,'f',3));
    model_amclDebugInfo->setItem(1,1,item);

    item = new QStandardItem("offset_y(m)");
    model_amclDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoOut.correctPositionOffset.y,'f',3));
    model_amclDebugInfo->setItem(2,1,item);

    item = new QStandardItem("offset_r(degree)");
    model_amclDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoOut.correctPositionOffset.rotate*180.0/M_PI,'f',3));
    model_amclDebugInfo->setItem(3,1,item);

    item = new QStandardItem("correctCount");
    model_amclDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoOut.correctCount));
    model_amclDebugInfo->setItem(4,1,item);



    ui->tableView_amclDebugInfo->setModel(model_amclDebugInfo);
    ui->tableView_amclDebugInfo->resizeColumnToContents(0);
    ui->tableView_amclDebugInfo->resizeColumnToContents(1);
    ui->tableView_amclDebugInfo->resizeColumnToContents(2);
    ui->tableView_amclDebugInfo->show();
}

int MotionForm::displayQrcodeDebugInfo()
{
    QrcodeDebugInfo infoOut;
    mcInterpreter->getQrcodeDebugInfo(infoOut);

    model_qrcodeDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;


    item = new QStandardItem("x");
    model_qrcodeDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoOut.positionFeedback.x,'f',4));
    model_qrcodeDebugInfo->setItem(0,1,item);

    item = new QStandardItem("y");
    model_qrcodeDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoOut.positionFeedback.y,'f',4));
    model_qrcodeDebugInfo->setItem(1,1,item);

    item = new QStandardItem("r(degree)");
    model_qrcodeDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoOut.positionFeedback.rotate*180.0/M_PI,'f',3));
    model_qrcodeDebugInfo->setItem(2,1,item);

    item = new QStandardItem("sensorId");
    model_qrcodeDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoOut.sensorId));
    model_qrcodeDebugInfo->setItem(3,1,item);

    item = new QStandardItem("isDeviceOpened");
    model_qrcodeDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoOut.isDeviceOpened));
    model_qrcodeDebugInfo->setItem(4,1,item);


    item = new QStandardItem("isImageAcquired");
    model_qrcodeDebugInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoOut.isImageAcquired));
    model_qrcodeDebugInfo->setItem(5,1,item);


    item = new QStandardItem("featureSqureCount");
    model_qrcodeDebugInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoOut.featureSqureCount));
    model_qrcodeDebugInfo->setItem(6,1,item);

    item = new QStandardItem("minConsumed(ms)");
    model_qrcodeDebugInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoOut.minConsumed,'f',3));
    model_qrcodeDebugInfo->setItem(7,1,item);

    item = new QStandardItem("currentConsumed");
    model_qrcodeDebugInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoOut.currentConsumed,'f',3));
    model_qrcodeDebugInfo->setItem(8,1,item);

    item = new QStandardItem("maxConsumed");
    model_qrcodeDebugInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoOut.maxConsumed,'f',3));
    model_qrcodeDebugInfo->setItem(9,1,item);

    ui->tableView_qrcodeDebugInfo->setModel(model_qrcodeDebugInfo);
    ui->tableView_qrcodeDebugInfo->resizeColumnToContents(0);
    ui->tableView_qrcodeDebugInfo->resizeColumnToContents(1);
    ui->tableView_qrcodeDebugInfo->resizeColumnToContents(2);
    ui->tableView_qrcodeDebugInfo->show();
}

int MotionForm::displayVehicleVelCommand()
{
    double com_xMove,com_yMove,com_rotate;
    mcInterpreter->getVehicleVelCommand(false,com_xMove,com_yMove,com_rotate);
    ui->lineEdit_autoVelCommandX->setText(QString::number(com_xMove,'f',3));
    ui->lineEdit_autoVelCommandY->setText(QString::number(com_yMove,'f',3));
    ui->lineEdit_autoVelCommandR->setText(QString::number(180*com_rotate/M_PI,'f',3));

    mcInterpreter->getVehicleVelCommand(true,com_xMove,com_yMove,com_rotate);
    ui->lineEdit_jogVelCommandX->setText(QString::number(com_xMove,'f',3));
    ui->lineEdit_jogVelCommandY->setText(QString::number(com_yMove,'f',3));
    ui->lineEdit_jogVelCommandR->setText(QString::number(180*com_rotate/M_PI,'f',3));
    return 1;
}

int MotionForm::displayVisionTrackDebugInfo()
{
    VisionTrackDebugInfo visionInfoOut;
    mcInterpreter->getVisionTrackDebugInfo(ui->comboBox_trackVisionIndex->currentIndex(),visionInfoOut);
    ui->lineEdit_visionTrackReceive_objectCount->setText(QString::number(visionInfoOut.cameraData.pointCount));
    ui->lineEdit_visionTrackReceive_x->setText(QString::number(visionInfoOut.cameraData.x,'f',3));
    ui->lineEdit_visionTrackReceive_y->setText(QString::number(visionInfoOut.cameraData.y,'f',3));
    ui->lineEdit_visionTrackReceive_r->setText(QString::number(visionInfoOut.cameraData.r,'f',3));
    QString connectInfo;
    if(visionInfoOut.isConnectedStatus)
    {
        connectInfo="已经成功连接";
    }
    else
    {
        connectInfo="失败：错误码";
        connectInfo+=QString::number(visionInfoOut.socketErrorCode);
    }
    ui->lineEdit_visionTrack_cameraStatus->setText(connectInfo);
    ui->lineEdit_visionObjectCount->setText(QString::number(visionInfoOut.objectCount));
    return 1;
}

int MotionForm::displayBeltTrackDebugInfo()
{
    BeltTrackDebugInfo beltTrackInfoOut;
    if(1!=mcInterpreter->getBeltTrackDebugInfo(ui->comboBox_trackBeltIndex->currentIndex(),beltTrackInfoOut))
    {
        model_beltTrackDebugInfo->clear();
        return 0;
    }

    model_beltTrackDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("objectSize");
    model_beltTrackDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.objectSize));
    model_beltTrackDebugInfo->setItem(0,1,item);

    item = new QStandardItem("okSize");
    model_beltTrackDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.okSize));
    model_beltTrackDebugInfo->setItem(1,1,item);

    item = new QStandardItem("MovedLength");
    model_beltTrackDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.currentMovedLength,'f',4));
    model_beltTrackDebugInfo->setItem(2,1,item);

    item = new QStandardItem("movingLength");
    model_beltTrackDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.movingLength,'f',4));
    model_beltTrackDebugInfo->setItem(3,1,item);



    item = new QStandardItem("isTracking");
    model_beltTrackDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.isTracking));
    model_beltTrackDebugInfo->setItem(4,1,item);


    item = new QStandardItem("isMoveValid");
    model_beltTrackDebugInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.isMoveValidOut));
    model_beltTrackDebugInfo->setItem(5,1,item);

    item = new QStandardItem("poseFeedback");
    model_beltTrackDebugInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.poseFeedback,'f',1));
    model_beltTrackDebugInfo->setItem(6,1,item);

    item = new QStandardItem("poseCommand");
    model_beltTrackDebugInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.poseCommandOut,'f',1));
    model_beltTrackDebugInfo->setItem(7,1,item);

    item = new QStandardItem("acceleration");
    model_beltTrackDebugInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.acceleration,'f',1));
    model_beltTrackDebugInfo->setItem(8,1,item);

    item = new QStandardItem("holdPose");
    model_beltTrackDebugInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.holdPose,'f',1));
    model_beltTrackDebugInfo->setItem(9,1,item);

    item = new QStandardItem("moveStage");
    model_beltTrackDebugInfo->setItem(10,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.moveStage));
    model_beltTrackDebugInfo->setItem(10,1,item);

    item = new QStandardItem("beltVelocity");
    model_beltTrackDebugInfo->setItem(11,0,item);
    item = new QStandardItem(QString::number(beltTrackInfoOut.beltVelocity,'f',4));
    model_beltTrackDebugInfo->setItem(11,1,item);


    ui->tableView_beltTrackDebugInfo->setModel(model_beltTrackDebugInfo);
    ui->tableView_beltTrackDebugInfo->resizeColumnToContents(0);
    ui->tableView_beltTrackDebugInfo->resizeColumnToContents(1);
    ui->tableView_beltTrackDebugInfo->resizeColumnToContents(2);
    ui->tableView_beltTrackDebugInfo->show();



}

int MotionForm::displayRegulatorDebugInfo()
{
    RegulatorDebugInfo debugInfoOut;
    if(1!=mcInterpreter->getPositionRegulatorDebugInfo(ui->lineEdit_regulatorId->text().toInt(),debugInfoOut))
    {
        model_regulatorDebugInfo->clear();
        return 0;
    }

    model_regulatorDebugInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("axisId");
    model_regulatorDebugInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.axisId));
    model_regulatorDebugInfo->setItem(0,1,item);

    item = new QStandardItem("deltaTime");
    model_regulatorDebugInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.deltaTime));
    model_regulatorDebugInfo->setItem(1,1,item);

    item = new QStandardItem("status");
    model_regulatorDebugInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.status));
    model_regulatorDebugInfo->setItem(2,1,item);

    item = new QStandardItem("regulatorType");
    model_regulatorDebugInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.regulatorType));
    model_regulatorDebugInfo->setItem(3,1,item);

    item = new QStandardItem("targetPosition");
    model_regulatorDebugInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.targetPosition));
    model_regulatorDebugInfo->setItem(4,1,item);

    item = new QStandardItem("feedbackPosition");
    model_regulatorDebugInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.feedbackPosition));
    model_regulatorDebugInfo->setItem(5,1,item);

    item = new QStandardItem("velCommand");
    model_regulatorDebugInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.velCommand));
    model_regulatorDebugInfo->setItem(6,1,item);

    item = new QStandardItem("isValid");
    model_regulatorDebugInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.isValid));
    model_regulatorDebugInfo->setItem(7,1,item);

    item = new QStandardItem("feedforward");
    model_regulatorDebugInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.feedforward));
    model_regulatorDebugInfo->setItem(8,1,item);

    item = new QStandardItem("propotial");
    model_regulatorDebugInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.propotial));
    model_regulatorDebugInfo->setItem(9,1,item);

    item = new QStandardItem("integration");
    model_regulatorDebugInfo->setItem(10,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.integration));
    model_regulatorDebugInfo->setItem(10,1,item);

    item = new QStandardItem("errorLimitForInt");
    model_regulatorDebugInfo->setItem(11,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.errorLimitForIntegration));
    model_regulatorDebugInfo->setItem(11,1,item);

    item = new QStandardItem("differential");
    model_regulatorDebugInfo->setItem(12,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.differential));
    model_regulatorDebugInfo->setItem(12,1,item);

    item = new QStandardItem("minOutput");
    model_regulatorDebugInfo->setItem(13,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.minOutput));
    model_regulatorDebugInfo->setItem(13,1,item);

    item = new QStandardItem("maxOutput");
    model_regulatorDebugInfo->setItem(14,0,item);
    item = new QStandardItem(QString::number(debugInfoOut.configInfo.maxOutput));
    model_regulatorDebugInfo->setItem(14,1,item);


    ui->tableView_regulatorDebugInfo->setModel(model_regulatorDebugInfo);
    ui->tableView_regulatorDebugInfo->resizeColumnToContents(0);
    ui->tableView_regulatorDebugInfo->resizeColumnToContents(1);
    ui->tableView_regulatorDebugInfo->resizeColumnToContents(2);
    ui->tableView_regulatorDebugInfo->show();

}


void MotionForm::on_pushButton_2_clicked()
{
    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;
    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=30;
    targetPoint.positionValue[1]=40;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=60;
    targetPoint.positionValue[4]=130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT_SPLINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 0.5;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;

    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);
    mcInterpreter->allStartRun();


    QMessageBox::information(NULL, "joint run test", "joint run test", QMessageBox::Yes );
}
void MotionForm::on_pushButton_12_clicked()
{
    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;



    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-30;
    targetPoint.positionValue[1]=-40;
    targetPoint.positionValue[2]=-50;
    targetPoint.positionValue[3]=-60;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT_SPLINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 0.5;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);
    mcInterpreter->allStartRun();

}


void MotionForm::on_pushButton_13_clicked()
{
    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;



    targetPoint.jointFlag=JOINT_FLAG_JOINT;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-80;
    targetPoint.positionValue[1]=-90;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=-60;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_JOINT_SPLINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 0.5;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);

    mcInterpreter->allStartRun();
}


//LINE
void MotionForm::on_pushButton_3_clicked()
{
    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;

    targetPoint.jointFlag=JOINT_FLAG_CARTESIAN;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-80;
    targetPoint.positionValue[1]=-90;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=-60;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_LINE;
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 0.5;
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);

    mcInterpreter->allStartRun();
}

void MotionForm::showWaveForm()
{
    motorView->setGeometry(50,300,820,700);

    motorView->show();

//    motorView->startWave();默认不要开启

}

int MotionForm::setDebugFlag(int debugFlagIn)
{
    debugFlag=debugFlagIn;
    motorView->setDebugFlag(debugFlagIn);
    return 1;
}

void MotionForm::on_pushButton_4_clicked()
{

    motorView->show();
}

void MotionForm::on_pushButton_5_clicked()
{
//    mcInterpreter->changeDragMode(1);
    mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_DRAG);
    QMessageBox::information(NULL, "Title", "drag on", QMessageBox::Yes, QMessageBox::Yes);
}

void MotionForm::on_pushButton_6_clicked()
{
//    mcInterpreter->changeDragMode(0);
    mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
    QMessageBox::information(NULL, "Title", "exit drag", QMessageBox::Yes , QMessageBox::Yes);
}

void MotionForm::on_pushButton_7_clicked()
{
    motorView->receiveWaveData(true);
}

void MotionForm::on_pushButton_8_clicked()
{
//    mcInterpreter->setVelocityOffsetMode(1);
    QMessageBox::information(NULL, "Title", "VelocityOffsetMode", QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
}

void MotionForm::on_pushButton_9_clicked()
{
//    mcInterpreter->setVelocityOffsetMode(0);
}

void MotionForm::on_pushButton_10_clicked()
{
//    mcInterpreter->setTorqueOffsetMode(1);
}

void MotionForm::on_pushButton_11_clicked()
{
//    mcInterpreter->setTorqueOffsetMode(0);
}

//JOG
void MotionForm::on_pushButton_clicked()
{
    mcInterpreter->jog(0,1,90);
}





//multi point joint move
void MotionForm::on_pushButton_15_clicked()
{


    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;

    targetPoint.jointFlag=JOINT_FLAG_CARTESIAN;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-80;
    targetPoint.positionValue[1]=0;
    targetPoint.positionValue[2]=0;
    targetPoint.positionValue[3]=0;
    targetPoint.positionValue[4]=0;
    targetPoint.positionValue[5]=0;
    targetPoint.positionValue[6]=0;

    targetPoint.extPositionValue.resize(0);

    tmpMovePara.moveType = ENUM_COMMAND_JOINT_CONTINIOUS;//
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity = 7;//-1时，用各个轴的最大速度，正数时，表示路径时间
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;//暂时没起到作用
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);

    //point 2
    targetPoint.positionValue[0]=-50;
    targetPoint.positionValue[1]=30;
    targetPoint.positionValue[2]=0;
    targetPoint.positionValue[3]=0;
    targetPoint.positionValue[4]=0;
    targetPoint.positionValue[5]=0;
    targetPoint.positionValue[6]=0;
    tmpMovePara.targetPoint = targetPoint;

    motionParaList.append(tmpMovePara);

    //point 3
    targetPoint.positionValue[0]=0;
    targetPoint.positionValue[1]=80;
    targetPoint.positionValue[2]=90;
    targetPoint.positionValue[3]=-50;
    targetPoint.positionValue[4]=30;
    targetPoint.positionValue[5]=30;
    targetPoint.positionValue[6]=-90;
    tmpMovePara.targetPoint = targetPoint;

    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);

    mcInterpreter->allStartRun();

}
//multi point cartision move
void MotionForm::on_pushButton_16_clicked()
{


    QVector <MoveParameter> motionParaList;
    MoveParameter tmpMovePara;
    PointPro targetPoint;

    targetPoint.jointFlag=JOINT_FLAG_CARTESIAN;
    targetPoint.pointNum=9;
    targetPoint.groupNum=1;
    targetPoint.ufValue=0;
    targetPoint.utValue=0;
    targetPoint.recordStatus=1;
    targetPoint.configString="r, t,s ,1,1,1";
    targetPoint.configInt.resize(7);
    targetPoint.positionValue.resize(7);
    targetPoint.positionValue[0]=-80;
    targetPoint.positionValue[1]=-90;
    targetPoint.positionValue[2]=50;
    targetPoint.positionValue[3]=-60;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;

    targetPoint.extPositionValue.resize(0);


    tmpMovePara.moveType = ENUM_COMMAND_CARTESIAN_CONTINIOUS;//
    tmpMovePara.velocityUnit = ENUM_RATIO;
    tmpMovePara.velocity =2;//-1时，用各个轴的最大速度，正数时，表示路径时间
    tmpMovePara.pointType = ENUM_NORMAL_POINT;
    tmpMovePara.targetPoint = targetPoint;
    tmpMovePara.index = 3;
    tmpMovePara.cntRatio = 20;
    tmpMovePara.acc = 65;//暂时没起到作用
    tmpMovePara.lineNumber = 7;


    motionParaList.append(tmpMovePara);

    //point 2
    targetPoint.positionValue[0]=-50;
    targetPoint.positionValue[1]=30;
    targetPoint.positionValue[2]=70;
    targetPoint.positionValue[3]=0;
    targetPoint.positionValue[4]=-130;
    targetPoint.positionValue[5]=-30;
    targetPoint.positionValue[6]=90;
    tmpMovePara.targetPoint = targetPoint;

    motionParaList.append(tmpMovePara);

    //point 3
    targetPoint.positionValue[0]=0;
    targetPoint.positionValue[1]=80;
    targetPoint.positionValue[2]=90;
    targetPoint.positionValue[3]=-50;
    targetPoint.positionValue[4]=30;
    targetPoint.positionValue[5]=30;
    targetPoint.positionValue[6]=-90;
    tmpMovePara.targetPoint = targetPoint;

    motionParaList.append(tmpMovePara);


    mcInterpreter->addTrajectoryCommand(motionParaList);

    mcInterpreter->allStartRun();

}

//cease
void MotionForm::on_pushButton_17_clicked()
{
    mcInterpreter->allCease();
}

//STOP
void MotionForm::on_pushButton_14_clicked()
{
    mcInterpreter->allStop();
}

void MotionForm::on_pushButton_18_clicked()
{
    //start move
    mcInterpreter->allStartRun();
}

void MotionForm::on_pushButton_19_clicked()
{
    //t2 check mode
    mcInterpreter->setCheckMode(E_CHECK_MODE_T2);
}

void MotionForm::on_pushButton_20_clicked()
{
    ui->checkBox->setChecked(false);
    mcInterpreter->resetAll();
}



void MotionForm::on_pushButton_22_clicked()
{
    mcInterpreter->setVehicleIdealSpeed_ratio(ui->lineEdit_11->text().toDouble());
}

void MotionForm::on_checkBox_2_clicked(bool checked)
{



}

void MotionForm::on_pushButton_23_clicked()
{
    mcInterpreter->resetVehicleMagneticMove();
}

void MotionForm::on_comboBox_currentIndexChanged(int index)
{
    if(0==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
    }
    else if(1==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_TRAJECTORY_COMMAND_PROCESS);
    }
    else if(2==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK);

    }
    else if(3==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_JOG);

    }
    else if(4==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL);

    }
    else if(5==index)
    {
        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);

    }
    else if(6==index)
    {
//        mcInterpreter->setMotionServerModel(ENUM_MOTION_MODEL_DRAG);

    }

}

void MotionForm::on_pushButton_24_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改磁条跟踪pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    //磁条追踪设置
    TrackPidInfo pidInfo;
    pidInfo.errorLimitForIntegration=ui->lineEdit_maganetic_errorLimitForIntegration->text().toDouble();
    pidInfo.feedforward=ui->lineEdit_maganetic_feedforward->text().toDouble();
    pidInfo.lineAdjustMax=ui->lineEdit_maganetic_pid_lineAdjust->text().toDouble();
    pidInfo.rotationAdjustMax=ui->lineEdit_maganetic_pid_angleAdjust->text().toDouble();
    pidInfo.pid_d=ui->lineEdit_maganetic_pid_d->text().toDouble();
    pidInfo.pid_i=ui->lineEdit_maganetic_pid_i->text().toDouble();
    pidInfo.pid_p=ui->lineEdit_maganetic_pid_p->text().toDouble();
    pidInfo.pid_velocity_p=ui->lineEdit_maganetic_pid_velp->text().toDouble();
    mcInterpreter->setVehicleMagneticMovePid(0,pidInfo);
}

void MotionForm::on_pushButton_21_clicked()
{
    //up

    leftSpeedCommand+=1*ui->horizontalSlider_2->value();
    rightSpeedCommand+=1*ui->horizontalSlider_2->value();
}

void MotionForm::on_pushButton_25_clicked()
{
    //down
    leftSpeedCommand-=1*ui->horizontalSlider_2->value();
    rightSpeedCommand-=1*ui->horizontalSlider_2->value();

}

void MotionForm::on_pushButton_26_clicked()
{
    //turn left
    leftSpeedCommand-=1*ui->horizontalSlider_2->value();
    rightSpeedCommand+=1*ui->horizontalSlider_2->value();
}

void MotionForm::on_pushButton_27_clicked()
{
    //turn right
    leftSpeedCommand+=1*ui->horizontalSlider_2->value();
    rightSpeedCommand-=1*ui->horizontalSlider_2->value();
}

void MotionForm::on_pushButton_28_clicked()
{

}



int MotionForm::vehicleJog()
{
    QString tmpString;
    tmpString=tmpString+"X="+QString::number(ballJoyStick->getX(),'f',2)+
            "Y="+QString::number(ballJoyStick->getY(),'f',2);
    ui->label_3->setText(tmpString);

    mcInterpreter->vehicleJog(-ballJoyStick->getY()*ui->horizontalSlider->value()/100.0,0,
                              -ballJoyStick->getX()*ui->horizontalSlider->value()/100.0);
}



void MotionForm::on_checkBox_timer_clicked(bool checked)
{
    if(checked)
    {
        timer->start(300);
    }
    else
    {
        timer->stop();
    }
}

void MotionForm::on_pushButton_getMessage_clicked()
{
//    mcInterpreter->getMe
}

void MotionForm::on_pushButton_getVehicleVel_clicked()
{

}

void MotionForm::on_checkBox_isVehicleJognew_clicked(bool checked)
{
    //vehicle jog
    if(true==checked)
    {
        ballJoyStick->move(100,200);
        ballJoyStick->show();
        mcInterpreter->startVehicleJogMove();
    }
    else
    {
        ballJoyStick->hide();
        mcInterpreter->stopVehicleJogMove();

    }
}

void MotionForm::on_pushButton_startNavatation_clicked()
{
    mcInterpreter->startNavigation();
}

void MotionForm::on_pushButton_stopNavagation_clicked()
{
    mcInterpreter->stopNavigation();
}

void MotionForm::on_pushButton_rfid_write_clicked()
{
    //sensor display
    int aliasIn=ui->comboBox_rfid_aliasIn->currentText().toInt();
//    QVector<unsigned char> cardValueIn;
//    unsigned char tmpUC;
    long long dataIn=ui->lineEdit_RFID_write_data->text().toLongLong();
//    tmpUC=dataIn&0xff;
//    cardValueIn.append(tmpUC);
//    tmpUC=(dataIn>>8)&0xff;
//    cardValueIn.append(tmpUC);
//    tmpUC=(dataIn>>16)&0xff;
//    cardValueIn.append(tmpUC);
//    tmpUC=(dataIn>>24)&0xff;
//    cardValueIn.append(tmpUC);

    int returnKey=mcInterpreter->writeTagData(aliasIn,dataIn);

}

void MotionForm::on_comboBox_maganetic_mode_currentIndexChanged(int index)
{
    mcInterpreter->setTrackMode((E_TRACK_MODE)index);
}

void MotionForm::on_comboBox_maganetic_stop_mode_currentIndexChanged(int index)
{
    mcInterpreter->setTrackStopCondition((E_TRACK_STOP_CONDITION)index);
}

void MotionForm::on_pushButton_startMaganeticTrack_clicked()
{
    mcInterpreter->startVehicleMagneticMove();
}

void MotionForm::on_pushButton_setTrackStopInput_clicked()
{
    mcInterpreter->setMaganeticInputIndex(ui->lineEdit_stopInput->text().toInt());
}

int MotionForm::showDebugInfo_matchInfo(PclMatchDebugInfo matchInfo)
{
    model_matchInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("isMatchSucess");
    model_matchInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(matchInfo.isMatchSucess));
    model_matchInfo->setItem(0,1,item);

    item = new QStandardItem("method");
    model_matchInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(matchInfo.method));
    model_matchInfo->setItem(1,1,item);

    item = new QStandardItem("consumeTime(ms)");
    model_matchInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(matchInfo.consumeTimeOut/1000.0,'f',0));
    model_matchInfo->setItem(2,1,item);

    item = new QStandardItem("maximumIterations");
    model_matchInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(matchInfo.maximumIterations,'f',2));
    model_matchInfo->setItem(3,1,item);

    item = new QStandardItem("transformationEpsilon");
    model_matchInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(matchInfo.transformationEpsilon));
    model_matchInfo->setItem(4,1,item);

    item = new QStandardItem("euclideanFitnessEpsilon");
    model_matchInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(matchInfo.euclideanFitnessEpsilon));
    model_matchInfo->setItem(5,1,item);

    item = new QStandardItem("matchScore");
    model_matchInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(matchInfo.matchScore,'f',3));
    model_matchInfo->setItem(6,1,item);

    item = new QStandardItem("pos_x");
    model_matchInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(matchInfo.pos_x,'f',3));
    model_matchInfo->setItem(7,1,item);

    item = new QStandardItem("pos_y");
    model_matchInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(matchInfo.pos_y,'f',3));
    model_matchInfo->setItem(8,1,item);

    item = new QStandardItem("pos_r");
    model_matchInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(matchInfo.pos_r,'f',3));
    model_matchInfo->setItem(9,1,item);

//    item = new QStandardItem("baseCompareRows");
//    model_matchInfo->setItem(10,0,item);
//    item = new QStandardItem(QString::number(matchInfo.baseCompareRows));
//    model_matchInfo->setItem(10,1,item);

//    item = new QStandardItem("localCompareCols");
//    model_matchInfo->setItem(11,0,item);
//    item = new QStandardItem(QString::number(matchInfo.localCompareCols));
//    model_matchInfo->setItem(11,1,item);

//    item = new QStandardItem("localCompareRows");
//    model_matchInfo->setItem(12,0,item);
//    item = new QStandardItem(QString::number(matchInfo.localCompareRows));
//    model_matchInfo->setItem(12,1,item);


    ui->tableView_matchDebugInfo->setModel(model_matchInfo);
    ui->tableView_matchDebugInfo->resizeColumnToContents(0);
    ui->tableView_matchDebugInfo->resizeColumnToContents(1);
    ui->tableView_matchDebugInfo->resizeColumnToContents(2);
    ui->tableView_matchDebugInfo->show();
}

int MotionForm::showRosApiTimeConsume()
{
    double minConsumedOut_amcl;
    double currentConsumedOut_amcl;
    double maxConsumedOut_amcl;
    double minConsumedOut_laserOdom;
    double currentConsumedOut_laserOdom;
    double maxConsumedOut_laserOdom;
    double minConsumedOut_pointCloudConsumed;
    double currentConsumedOut_pointCloudConsumed;
    double maxConsumedOut_pointCloudConsumed;
    double minConsumedOut_vmark;
    double currentConsumedOut_vmark;
    double maxConsumedOut_vmark;
    double minConsumedOut_cloudTrans;
    double currentConsumedOut_cloudTrans;
    double maxConsumedOut_cloudTrans;
    double minConsumedOut_rosThread;
    double currentConsumedOut_rosThread;
    double maxConsumedOut_rosThread;
    mcInterpreter->getTimeConsumed(1,minConsumedOut_amcl,currentConsumedOut_amcl,maxConsumedOut_amcl);
    mcInterpreter->getTimeConsumed(2,minConsumedOut_laserOdom,currentConsumedOut_laserOdom,maxConsumedOut_laserOdom);
    mcInterpreter->getTimeConsumed(3,minConsumedOut_vmark,currentConsumedOut_vmark,maxConsumedOut_vmark);
    mcInterpreter->getTimeConsumed(4,minConsumedOut_cloudTrans,currentConsumedOut_cloudTrans,maxConsumedOut_cloudTrans);
     mcInterpreter->getTimeConsumed(5,minConsumedOut_rosThread,currentConsumedOut_rosThread,maxConsumedOut_rosThread);
    mcInterpreter->getTimeConsumed_pointCloudDetect(minConsumedOut_pointCloudConsumed,currentConsumedOut_pointCloudConsumed,
                                                    maxConsumedOut_pointCloudConsumed);

    model_timeConsumeInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("amcl_currentTime(ms)");
    model_timeConsumeInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_amcl/1000.0,'f',3));
    model_timeConsumeInfo->setItem(0,1,item);

    item = new QStandardItem("amcl_minTime");
    model_timeConsumeInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_amcl/1000.0,'f',3));
    model_timeConsumeInfo->setItem(1,1,item);

    item = new QStandardItem("amcl_maxTime");
    model_timeConsumeInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_amcl/1000.0,'f',3));
    model_timeConsumeInfo->setItem(2,1,item);

    item = new QStandardItem("rosThread(ms)");
    model_timeConsumeInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_rosThread/1000.0,'f',3));
    model_timeConsumeInfo->setItem(3,1,item);

    item = new QStandardItem("rosThread_min");
    model_timeConsumeInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_rosThread/1000.0,'f',3));
    model_timeConsumeInfo->setItem(4,1,item);

    item = new QStandardItem("rosThread_max");
    model_timeConsumeInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_rosThread/1000.0,'f',3));
    model_timeConsumeInfo->setItem(5,1,item);

    item = new QStandardItem("点云避障(ms)");
    model_timeConsumeInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_pointCloudConsumed/1000.0,'f',3));
    model_timeConsumeInfo->setItem(6,1,item);

    item = new QStandardItem("点云避障_minTime");
    model_timeConsumeInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_pointCloudConsumed/1000.0,'f',3));
    model_timeConsumeInfo->setItem(7,1,item);

    item = new QStandardItem("点云避障_maxTime");
    model_timeConsumeInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_pointCloudConsumed/1000.0,'f',3));
    model_timeConsumeInfo->setItem(8,1,item);

    item = new QStandardItem("V槽");
    model_timeConsumeInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_vmark/1000.0,'f',3));
    model_timeConsumeInfo->setItem(9,1,item);

    item = new QStandardItem("V槽_min(ms)");
    model_timeConsumeInfo->setItem(10,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_vmark/1000.0,'f',3));
    model_timeConsumeInfo->setItem(10,1,item);

    item = new QStandardItem("V槽_max");
    model_timeConsumeInfo->setItem(11,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_vmark/1000.0,'f',3));
    model_timeConsumeInfo->setItem(11,1,item);

    item = new QStandardItem("点云变换");
    model_timeConsumeInfo->setItem(12,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_cloudTrans/1000.0,'f',3));
    model_timeConsumeInfo->setItem(12,1,item);

    item = new QStandardItem("点云变换_min");
    model_timeConsumeInfo->setItem(13,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_cloudTrans/1000.0,'f',3));
    model_timeConsumeInfo->setItem(13,1,item);

    item = new QStandardItem("点云变换_max");
    model_timeConsumeInfo->setItem(14,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_cloudTrans/1000.0,'f',3));
    model_timeConsumeInfo->setItem(14,1,item);

    item = new QStandardItem("odom_currentTime(ms)");
    model_timeConsumeInfo->setItem(15,0,item);
    item = new QStandardItem(QString::number(currentConsumedOut_laserOdom/1000.0,'f',3));
    model_timeConsumeInfo->setItem(15,1,item);

    item = new QStandardItem("odom_minTime");
    model_timeConsumeInfo->setItem(16,0,item);
    item = new QStandardItem(QString::number(minConsumedOut_laserOdom/1000.0,'f',3));
    model_timeConsumeInfo->setItem(16,1,item);

    item = new QStandardItem("odom_maxTime");
    model_timeConsumeInfo->setItem(17,0,item);
    item = new QStandardItem(QString::number(maxConsumedOut_laserOdom/1000.0,'f',3));
    model_timeConsumeInfo->setItem(17,1,item);


    ui->tableView_rosApiTimeConsume->setModel(model_timeConsumeInfo);
    ui->tableView_rosApiTimeConsume->resizeColumnToContents(0);
    ui->tableView_rosApiTimeConsume->resizeColumnToContents(1);
    ui->tableView_rosApiTimeConsume->resizeColumnToContents(2);
    ui->tableView_rosApiTimeConsume->show();
}

int MotionForm::showAlgroCallTime()
{
    vector<string> comment;
    vector<double> timeInfo;
    mcInterpreter->getAlgroCallTimeMeasureDebug(comment,timeInfo);

    model_algroCallTimeInfo->clear();
    //定义item
    QStandardItem* item = 0;
    for(int i=0;i<comment.size();i++)
    {
        item = new QStandardItem(QString::fromStdString(comment[i]));
        model_algroCallTimeInfo->setItem(0+i*3,0,item);
        item = new QStandardItem(QString::number(timeInfo[0+i*3]/1000.0,'f',3));
        model_algroCallTimeInfo->setItem(0+i*3,1,item);

        item = new QStandardItem("max(us)");
        model_algroCallTimeInfo->setItem(1+i*3,0,item);
        item = new QStandardItem(QString::number(timeInfo[1+i*3]/1000.0,'f',3));
        model_algroCallTimeInfo->setItem(1+i*3,1,item);

        item = new QStandardItem("min(us)");
        model_algroCallTimeInfo->setItem(2+i*3,0,item);
        item = new QStandardItem(QString::number(timeInfo[2+i*3]/1000.0,'f',3));
        model_algroCallTimeInfo->setItem(2+i*3,1,item);
    }






    ui->tableView_algroCallTime->setModel(model_algroCallTimeInfo);
    ui->tableView_algroCallTime->resizeColumnToContents(0);
    ui->tableView_algroCallTime->resizeColumnToContents(1);
    ui->tableView_algroCallTime->resizeColumnToContents(2);
    ui->tableView_algroCallTime->show();
}

int MotionForm::displayCu1DebugInfo()
{
    int requestCountOut;
    double l1Out, l2Out, imageOffsetOut;
    bool isSucess;
    int errorcode;
    mcInterpreter->getCu1SensorRawData(requestCountOut,l1Out, l2Out, imageOffsetOut,isSucess,errorcode);
    ui->lineEdit_cu1Position_requestCount->setText(QString::number(requestCountOut));
    ui->lineEdit_cu1Position_l1->setText(QString::number(l1Out));
    ui->lineEdit_cu1Position_l2->setText(QString::number(l2Out));
    ui->lineEdit_cu1Position_imageOffset->setText(QString::number(imageOffsetOut));
    ui->lineEdit_cu1Position_errorCode->setText(QString::number(errorcode));
    if(true==isSucess)
    {
        ui->lineEdit_cu1Position_isSucess->setText("sucess");
    }
    else
    {
        ui->lineEdit_cu1Position_isSucess->setText("failed");
    }

    return 1;
}

int MotionForm::displayEncoderReceiver(EncoderReceiverData infoIn)
{
    model_encoderReceiverInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("doOutput");
    model_encoderReceiverInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoIn.doOutput));
    model_encoderReceiverInfo->setItem(0,1,item);

    item = new QStandardItem("Encoder0_Val");
    model_encoderReceiverInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoIn.Encoder0_Val));
    model_encoderReceiverInfo->setItem(1,1,item);

    item = new QStandardItem("Encoder1_Val");
    model_encoderReceiverInfo->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoIn.Encoder1_Val));
    model_encoderReceiverInfo->setItem(2,1,item);

    item = new QStandardItem("Encoder2_Val");
    model_encoderReceiverInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoIn.Encoder2_Val));
    model_encoderReceiverInfo->setItem(3,1,item);

    item = new QStandardItem("HighSpeed_IN0");
    model_encoderReceiverInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoIn.HighSpeed_IN0));
    model_encoderReceiverInfo->setItem(4,1,item);

    item = new QStandardItem("Ltc0_Finished");
    model_encoderReceiverInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoIn.Ltc0_Finished));
    model_encoderReceiverInfo->setItem(5,1,item);

    item = new QStandardItem("Ltc0_Encoder0Val");
    model_encoderReceiverInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoIn.Ltc0_Encoder0Val));
    model_encoderReceiverInfo->setItem(6,1,item);

    item = new QStandardItem("Ltc0_Encoder1Val");
    model_encoderReceiverInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoIn.Ltc0_Encoder1Val));
    model_encoderReceiverInfo->setItem(7,1,item);

    item = new QStandardItem("Ltc0_Encoder2Val");
    model_encoderReceiverInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoIn.Ltc0_Encoder2Val));
    model_encoderReceiverInfo->setItem(8,1,item);

    item = new QStandardItem("alias");
    model_encoderReceiverInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoIn.deviceAlias));
    model_encoderReceiverInfo->setItem(9,1,item);




    ui->tableView_encoderReceiverData->setModel(model_encoderReceiverInfo);
    ui->tableView_encoderReceiverData->resizeColumnToContents(0);
    ui->tableView_encoderReceiverData->resizeColumnToContents(1);
    ui->tableView_encoderReceiverData->resizeColumnToContents(2);
    ui->tableView_encoderReceiverData->show();
}

int MotionForm::displayImuData(ImuDataInfo infoIn)
{
    model_imuData->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("requestCount");
    model_imuData->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoIn.requestCount));
    model_imuData->setItem(0,1,item);

    item = new QStandardItem("roll");
    model_imuData->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoIn.roll));
    model_imuData->setItem(1,1,item);

    item = new QStandardItem("pitch");
    model_imuData->setItem(2,0,item);
    item = new QStandardItem(QString::number(infoIn.pitch));
    model_imuData->setItem(2,1,item);

    item = new QStandardItem("yaw");
    model_imuData->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoIn.yaw));
    model_imuData->setItem(3,1,item);

    item = new QStandardItem("rotateVel_x");
    model_imuData->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoIn.rotateVel_x));
    model_imuData->setItem(4,1,item);

    item = new QStandardItem("rotateVel_y");
    model_imuData->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoIn.rotateVel_y));
    model_imuData->setItem(5,1,item);

    item = new QStandardItem("rotateVel_z");
    model_imuData->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoIn.rotateVel_z));
    model_imuData->setItem(6,1,item);

    item = new QStandardItem("acc_x");
    model_imuData->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoIn.acc_x));
    model_imuData->setItem(7,1,item);

    item = new QStandardItem("acc_y");
    model_imuData->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoIn.acc_y));
    model_imuData->setItem(8,1,item);

    item = new QStandardItem("acc_z");
    model_imuData->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoIn.acc_z));
    model_imuData->setItem(9,1,item);


    ui->tableView_imuData->setModel(model_imuData);
    ui->tableView_imuData->resizeColumnToContents(0);
    ui->tableView_imuData->resizeColumnToContents(1);
    ui->tableView_imuData->resizeColumnToContents(2);
    ui->tableView_imuData->show();
}

int MotionForm::displayEmcDebugInfo(EmcDebugInfo &infoIn)
{
    model_emcTpInfo->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("queueSize");
    model_emcTpInfo->setItem(0,0,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.queueSize));
    model_emcTpInfo->setItem(0,1,item);
    item = new QStandardItem("cycleTime");
    model_emcTpInfo->setItem(0,2,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.cycleTime));
    model_emcTpInfo->setItem(0,3,item);
    item = new QStandardItem("vMax");
    model_emcTpInfo->setItem(0,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.vMax));
    model_emcTpInfo->setItem(0,5,item);
    item = new QStandardItem("ini_maxvel");
    model_emcTpInfo->setItem(0,6,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.ini_maxvel));
    model_emcTpInfo->setItem(0,7,item);
    item = new QStandardItem("vLimit");
    model_emcTpInfo->setItem(0,8,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.vLimit));
    model_emcTpInfo->setItem(0,9,item);

    item = new QStandardItem("aMaxCartesian");
    model_emcTpInfo->setItem(1,0,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.aMaxCartesian));
    model_emcTpInfo->setItem(1,1,item);
    item = new QStandardItem("wMax");
    model_emcTpInfo->setItem(1,2,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.wMax));
    model_emcTpInfo->setItem(1,3,item);
    item = new QStandardItem("wDotMax");
    model_emcTpInfo->setItem(1,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.wDotMax));
    model_emcTpInfo->setItem(1,5,item);
    item = new QStandardItem("nextId");
    model_emcTpInfo->setItem(1,6,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.nextId));
    model_emcTpInfo->setItem(1,7,item);
    item = new QStandardItem("execId");
    model_emcTpInfo->setItem(1,8,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.execId));
    model_emcTpInfo->setItem(1,9,item);



    item = new QStandardItem("尾型");
    model_emcTpInfo->setItem(2,0,item);
    QString tmpTermCond;
    switch(infoIn.tpInfo.termCond)
    {
    case 0:
        tmpTermCond="stop";
        break;
    case 1:
        tmpTermCond="EXACT";
        break;
    case 2:
        tmpTermCond="PARABOLIC";
        break;
    case 3:
        tmpTermCond="TANGENT";
        break;
    default:
        tmpTermCond=QString::number(infoIn.tpInfo.termCond);

    }
    item = new QStandardItem(tmpTermCond);
    model_emcTpInfo->setItem(2,1,item);
    item = new QStandardItem("done");
    model_emcTpInfo->setItem(2,2,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.done));
    model_emcTpInfo->setItem(2,3,item);
    item = new QStandardItem("depth");
    model_emcTpInfo->setItem(2,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.depth));
    model_emcTpInfo->setItem(2,5,item);
    item = new QStandardItem("activeDepth");
    model_emcTpInfo->setItem(2,6,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.activeDepth));
    model_emcTpInfo->setItem(2,7,item);
    item = new QStandardItem("aborting");
    model_emcTpInfo->setItem(2,8,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.aborting));
    model_emcTpInfo->setItem(2,9,item);



    item = new QStandardItem("pausing");
    model_emcTpInfo->setItem(3,0,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.pausing));
    model_emcTpInfo->setItem(3,1,item);
    item = new QStandardItem("motionType");
    model_emcTpInfo->setItem(3,2,item);
    QString tmpMotionType;
    switch(infoIn.tpInfo.motionType)
    {
    case 0:
        tmpMotionType="LINEAR";
        break;
    case 1:
        tmpMotionType="CIRCULAR";
        break;
    case 2:
        tmpMotionType="RIGIDTAP";
        break;
    case 3:
        tmpMotionType="SPHERICAL";
        break;
    default:
        tmpMotionType=QString::number(infoIn.tpInfo.motionType);

    }
    item = new QStandardItem(tmpMotionType);
    model_emcTpInfo->setItem(3,3,item);
    item = new QStandardItem("tolerance");
    model_emcTpInfo->setItem(3,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.tolerance));
    model_emcTpInfo->setItem(3,5,item);
    item = new QStandardItem("-");
    model_emcTpInfo->setItem(3,6,item);
    item = new QStandardItem(QString::number(0));
    model_emcTpInfo->setItem(3,7,item);
    item = new QStandardItem("tc尾形");
    model_emcTpInfo->setItem(3,8,item);
    switch(infoIn.tpInfo.termCond)
    {
    case 0:
        tmpTermCond="stop";
        break;
    case 1:
        tmpTermCond="EXACT";
        break;
    case 2:
        tmpTermCond="PARABOLIC";
        break;
    case 3:
        tmpTermCond="TANGENT";
        break;
    default:
        tmpTermCond=QString::number(infoIn.tcInfo.term_cond);

    }
    item = new QStandardItem(tmpTermCond);
    model_emcTpInfo->setItem(3,9,item);


    item = new QStandardItem("target");
    model_emcTpInfo->setItem(4,0,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.target));
    model_emcTpInfo->setItem(4,1,item);
    item = new QStandardItem("progress");
    model_emcTpInfo->setItem(4,2,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.progress));
    model_emcTpInfo->setItem(4,3,item);
    item = new QStandardItem("reqvel");
    model_emcTpInfo->setItem(4,4,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.reqvel));
    model_emcTpInfo->setItem(4,5,item);
    item = new QStandardItem("target_vel");
    model_emcTpInfo->setItem(4,6,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.target_vel));
    model_emcTpInfo->setItem(4,7,item);
    item = new QStandardItem("maxvel");
    model_emcTpInfo->setItem(4,8,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.maxvel));
    model_emcTpInfo->setItem(4,9,item);


    item = new QStandardItem("currentvel");
    model_emcTpInfo->setItem(5,0,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.currentvel));
    model_emcTpInfo->setItem(5,1,item);
    item = new QStandardItem("finalvel");
    model_emcTpInfo->setItem(5,2,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.finalvel));
    model_emcTpInfo->setItem(5,3,item);
    item = new QStandardItem("term_vel");
    model_emcTpInfo->setItem(5,4,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.term_vel));
    model_emcTpInfo->setItem(5,5,item);
    item = new QStandardItem("kink_vel");
    model_emcTpInfo->setItem(5,6,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.kink_vel));
    model_emcTpInfo->setItem(5,7,item);
    item = new QStandardItem("maxaccel");
    model_emcTpInfo->setItem(5,8,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.maxaccel));
    model_emcTpInfo->setItem(5,9,item);


    item = new QStandardItem("id");
    model_emcTpInfo->setItem(6,0,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.id));
    model_emcTpInfo->setItem(6,1,item);
    item = new QStandardItem("coords");
    model_emcTpInfo->setItem(6,2,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.coords));
    model_emcTpInfo->setItem(6,3,item);
    item = new QStandardItem("motion_type");
    model_emcTpInfo->setItem(6,4,item);

    switch(infoIn.tpInfo.motionType)
    {
    case 0:
        tmpMotionType="LINEAR";
        break;
    case 1:
        tmpMotionType="CIRCULAR";
        break;
    case 2:
        tmpMotionType="RIGIDTAP";
        break;
    case 3:
        tmpMotionType="SPHERICAL";
        break;
    default:
        tmpMotionType=QString::number(infoIn.tpInfo.motionType);

    }
    item = new QStandardItem(tmpMotionType);
    model_emcTpInfo->setItem(6,5,item);
    item = new QStandardItem("active");
    model_emcTpInfo->setItem(6,6,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.active));
    model_emcTpInfo->setItem(6,7,item);
    item = new QStandardItem("blend_vel");
    model_emcTpInfo->setItem(6,8,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.blend_vel));
    model_emcTpInfo->setItem(6,9,item);


    item = new QStandardItem("currentPos");
    model_emcTpInfo->setItem(7,0,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[0]));
    model_emcTpInfo->setItem(7,1,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[1]));
    model_emcTpInfo->setItem(7,2,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[2]));
    model_emcTpInfo->setItem(7,3,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[3]));
    model_emcTpInfo->setItem(7,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[4]));
    model_emcTpInfo->setItem(7,5,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[5]));
    model_emcTpInfo->setItem(7,6,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[6]));
    model_emcTpInfo->setItem(7,7,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[7]));
    model_emcTpInfo->setItem(7,8,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.currentPos[8]));
    model_emcTpInfo->setItem(7,9,item);

    item = new QStandardItem("goalPos");
    model_emcTpInfo->setItem(8,0,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[0]));
    model_emcTpInfo->setItem(8,1,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[1]));
    model_emcTpInfo->setItem(8,2,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[2]));
    model_emcTpInfo->setItem(8,3,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[3]));
    model_emcTpInfo->setItem(8,4,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[4]));
    model_emcTpInfo->setItem(8,5,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[5]));
    model_emcTpInfo->setItem(8,6,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[6]));
    model_emcTpInfo->setItem(8,7,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[7]));
    model_emcTpInfo->setItem(8,8,item);
    item = new QStandardItem(QString::number(infoIn.tpInfo.goalPos[8]));
    model_emcTpInfo->setItem(8,9,item);

    item = new QStandardItem("isInReqLimit");
    model_emcTpInfo->setItem(9,0,item);
    item = new QStandardItem(QString::number(infoIn.tcInfo.isInReqLimit));
    model_emcTpInfo->setItem(9,1,item);

    ui->tableView_emcTpDebugInfo->setModel(model_emcTpInfo);
//    ui->tableView_emcTpDebugInfo->resizeColumnToContents(0);
//    ui->tableView_emcTpDebugInfo->resizeColumnToContents(1);
//    ui->tableView_emcTpDebugInfo->resizeColumnToContents(2);

    ui->tableView_emcTpDebugInfo->show();
}

int MotionForm::displaySimpleTpInfo(SimpleTpInfo simpleTpInfoIn)
{
    model_simpleTp->clear();
    //定义item
    QStandardItem* item = 0;

    item = new QStandardItem("pos_cmd");
    model_simpleTp->setItem(0,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.pos_cmd));
    model_simpleTp->setItem(0,1,item);

    item = new QStandardItem("max_vel");
    model_simpleTp->setItem(1,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.max_vel));
    model_simpleTp->setItem(1,1,item);

    item = new QStandardItem("max_acc");
    model_simpleTp->setItem(2,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.max_acc));
    model_simpleTp->setItem(2,1,item);

    item = new QStandardItem("enable");
    model_simpleTp->setItem(3,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.enable));
    model_simpleTp->setItem(3,1,item);

    item = new QStandardItem("curr_pos");
    model_simpleTp->setItem(4,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.curr_pos));
    model_simpleTp->setItem(4,1,item);

    item = new QStandardItem("curr_vel");
    model_simpleTp->setItem(5,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.curr_vel));
    model_simpleTp->setItem(5,1,item);

    item = new QStandardItem("active");
    model_simpleTp->setItem(6,0,item);
    item = new QStandardItem(QString::number(simpleTpInfoIn.active));
    model_simpleTp->setItem(6,1,item);


    ui->tableView_simpleTp->setModel(model_simpleTp);
    ui->tableView_simpleTp->resizeColumnToContents(0);
    ui->tableView_simpleTp->resizeColumnToContents(1);
    ui->tableView_simpleTp->resizeColumnToContents(2);
    ui->tableView_simpleTp->show();
}


int MotionForm::showRosMovingStatus()
{
    E_GOAL_STATUS statusOut;
    mcInterpreter->getMovingStatus(statusOut);
    int tmpStatus=(int)statusOut;

    QString tmpStr;

     if(tmpStatus==0)
     {
         tmpStr="goal pending!";
     }
    else if(tmpStatus==1)
    {
        tmpStr="goal started!";
    }
    else if(tmpStatus==2)
    {
        tmpStr="goal preemmpted!";

    }
    else if(tmpStatus==3)
    {
        tmpStr="goal succeeded!";
    }
    else if(tmpStatus==4)
    {
        tmpStr="goal aborted!";
    }
    else if(tmpStatus==5)
    {
        tmpStr="goal rejected!";
    }
    else if(tmpStatus==6)
    {
        tmpStr="goal preempting!";
    }
    else if(tmpStatus==7)
    {
        tmpStr="goal recalling!";
    }
    else if(tmpStatus==8)
    {
        tmpStr="goal recalled!";
    }
    else if(tmpStatus==9)
    {
        tmpStr="goal lost!";
    }
     else
     {
         tmpStr="unknow state!";
     }
    ui->lineEdit_rosMovingStatus->setText(tmpStr);



}


void MotionForm::on_pushButton_moveToDp_virtualLine_clicked()
{
    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.moveType=E_NAV_COMMAND_LINE;
    tmpParameter.velocityUnit=ENUM_RATIO;
    tmpParameter.velocity=ui->lineEdit_virtualLineSpeedRaio->text().toDouble();
    tmpParameter.targetPoint.posX=ui->lineEdit_rosPosX_2->text().toDouble();
    tmpParameter.targetPoint.posY=ui->lineEdit_rosPosY_2->text().toDouble();
    tmpParameter.targetPoint.posR=ui->lineEdit_rosPosR_2->text().toDouble();
    tmpParameter.cntRatio=0;
    tmpParameter.lineNumber=5;
    tmpParameter.incFlag=0;
    tmpParameter.pathFlag=0;
    tmpParameter.isPositiveDirection=ui->checkBox_virtualLineDirection->isChecked();

    tmpCommandVector.append(tmpParameter);

    mcInterpreter->addVirtualLineTrajectoryCommand(tmpCommandVector);
}



void MotionForm::on_pushButton_moveToDp_2_clicked()
{

}

void MotionForm::on_pushButton_moveToDp_virtualLine_2_clicked()
{
    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.moveType=E_NAV_COMMAND_LINE;
    tmpParameter.velocityUnit=ENUM_RATIO;
    tmpParameter.velocity=ui->lineEdit_virtualLineSpeedRaio_2->text().toDouble();
    tmpParameter.targetPoint.posX=ui->lineEdit_rosPosX_3->text().toDouble();
    tmpParameter.targetPoint.posY=ui->lineEdit_rosPosY_3->text().toDouble();
    tmpParameter.targetPoint.posR=ui->lineEdit_rosPosR_3->text().toDouble();
    tmpParameter.cntRatio=0;
    tmpParameter.lineNumber=10;
    tmpParameter.incFlag=0;
    tmpParameter.pathFlag=0;
    tmpParameter.isPositiveDirection=ui->checkBox_virtualLineDirection_2->isChecked();

    tmpCommandVector.append(tmpParameter);

    mcInterpreter->addVirtualLineTrajectoryCommand(tmpCommandVector);
}

void MotionForm::on_pushButton_calculateVisionMatch_clicked()
{
    MapRegion baseRegionIn;
    baseRegionIn.originX=ui->lineEdit_baseOriginX->text().toDouble();
    baseRegionIn.originY=ui->lineEdit_baseOriginY->text().toDouble();
    baseRegionIn.height=ui->lineEdit_baseHeight->text().toDouble();
    baseRegionIn.width=ui->lineEdit_baseWidth->text().toDouble();
    MapRegion localRegionIn;
    localRegionIn.originX=ui->lineEdit_baseOriginX_2->text().toDouble();
    localRegionIn.originY=ui->lineEdit_baseOriginY_2->text().toDouble();
    localRegionIn.height=ui->lineEdit_baseHeight_2->text().toDouble();
    localRegionIn.width=ui->lineEdit_baseWidth_2->text().toDouble();

    mcInterpreter->calculateVisionMatchVehiclePosition(baseRegionIn,localRegionIn);

    PositionInfo positionInfoOut;
    mcInterpreter->getVehiclePosition_visionMatch(positionInfoOut);

    ui->lineEdit_x->setText(QString::number(positionInfoOut.position.x,'f',3));
    ui->lineEdit_y->setText(QString::number(positionInfoOut.position.y,'f',3));
    ui->lineEdit_r->setText(QString::number(positionInfoOut.position.rotate,'f',3));
    ui->lineEdit_matchScore->setText(QString::number(positionInfoOut.matchScore,'f',3));
    ui->lineEdit_maxError->setText(QString::number(positionInfoOut.maxMatchError,'f',3));
    if(true==positionInfoOut.isUpdateSucess)
    {
        ui->label_isSucess->setText("sucess");
    }
    else
    {
        ui->label_isSucess->setText("failed");
    }

}

void MotionForm::on_pushButton_setBaseCompareMap_clicked()
{
    MapRegion baseRegionIn;
    baseRegionIn.originX=ui->lineEdit_baseOriginX->text().toDouble();
    baseRegionIn.originY=ui->lineEdit_baseOriginY->text().toDouble();
    baseRegionIn.height=ui->lineEdit_baseHeight->text().toDouble();
    baseRegionIn.width=ui->lineEdit_baseWidth->text().toDouble();
    mcInterpreter->setBaseMapRegion_pclicp(baseRegionIn);
}

void MotionForm::on_pushButton_setLocalCompareMap_clicked()
{
    MapRegion localRegionIn;
    localRegionIn.originX=ui->lineEdit_baseOriginX_2->text().toDouble();
    localRegionIn.originY=ui->lineEdit_baseOriginY_2->text().toDouble();
    localRegionIn.height=ui->lineEdit_baseHeight_2->text().toDouble();
    localRegionIn.width=ui->lineEdit_baseWidth_2->text().toDouble();
    mcInterpreter->setLocalMapRegion_pclicp(localRegionIn);

//    visionMatch->setLaserPosInLocalMap(ui->lineEdit_x_2->text().toDouble(),
//                                     ui->lineEdit_y_2->text().toDouble(),
//                                     ui->lineEdit_r_2->text().toDouble());
}

void MotionForm::on_pushButton_drawVisionMatchWindow_clicked()
{
    bool is1=ui->checkBox_isShowBaseMap->isChecked();
    bool is2=ui->checkBox_isShowBaseCompareMap->isChecked();
    bool is3=ui->checkBox_isShowLocalMap->isChecked();
    bool is4=ui->checkBox_isShowLocalCompareMap->isChecked();
    mcInterpreter->showVisionMatchDebugWindow(is1,is2,is3,is4);
}

void MotionForm::on_pushButton_setLaserMinimumIntensity_clicked()
{
    double tmpNum=ui->lineEdit_laserIntensity->text().toDouble();
    double range=ui->lineEdit_laserMaxRange->text().toDouble();
    double minAngle=ui->lineEdit_laserIntensity_minangle->text().toDouble();
    double maxAngle=ui->lineEdit_laserIntensity_maxangle->text().toDouble();
    mcInterpreter->setLaserIntensityFilter(tmpNum,range,minAngle/180.0*M_PI,maxAngle/180.0*M_PI);
}

void MotionForm::on_pushButton_setMathCalculateParameter_clicked()
{
    PclCalculationParameter tmpPara;
    tmpPara.method=(E_PCL_ICP_METHOD)ui->comboBox_matchMehod->currentIndex();
    tmpPara.maximumIterations=ui->lineEdit_maxInterat->text().toInt();
    tmpPara.transformationEpsilon=ui->lineEdit_transEps->text().toDouble();
    tmpPara.euclideanFitnessEpsilon=ui->lineEdit_edcli->text().toDouble();
    tmpPara.maxCorrespondenceDistance=ui->lineEdit_matchCosspondeDistance->text().toDouble();

    mcInterpreter->setMatchCalculateParameter(tmpPara);


}

void MotionForm::on_checkBox_pclIcpWork_clicked()
{
    bool isWork=ui->checkBox_pclIcpWork->isChecked();
    mcInterpreter->setPclIcpWork(isWork);
}

void MotionForm::on_pushButton_forceUpdateAmclPartial_clicked()
{
    mcInterpreter->forceAmclUpdatePartial();
}

void MotionForm::on_pushButton_readRosPosition_clicked()
{

    E_VEHICLE_POSITION_TYPE typeIn=E_VEHICLE_POSITION_TYPE_ROS;
    PositionInfo positionReturn;
    mcInterpreter->getVehiclePosition_specified(positionReturn,typeIn);
    ui->lineEdit_icp_initial_x->setText(QString::number(positionReturn.position.x,'f',3));
    ui->lineEdit_icp_initial_y->setText(QString::number(positionReturn.position.y,'f',3));
    ui->lineEdit_icp_initial_r->setText(QString::number(positionReturn.position.rotate,'f',3));
}

void MotionForm::on_pushButton_icpInitialPosition_clicked()
{
    double x=ui->lineEdit_icp_initial_x->text().toDouble();
    double y=ui->lineEdit_icp_initial_y->text().toDouble();
    double r=ui->lineEdit_icp_initial_r->text().toDouble();
    mcInterpreter->setVehiclePoseEstimation(x,y,r);
}

void MotionForm::on_pushButton_setVirtualPidMoveX_clicked()
{ 
    QMessageBox message(QMessageBox::Question, "提示", "是否修改前进控制pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    TrackPidInfo pidInfo;
    pidInfo.errorLimitForIntegration=ui->lineEdit_virtualLineX_errorLimitForIntegration->text().toDouble();
    pidInfo.feedforward=ui->lineEdit_virtualLineX_feedforward->text().toDouble();
    pidInfo.lineAdjustMax=0;
    pidInfo.rotationAdjustMax=0;
    pidInfo.pid_d=ui->lineEdit_moveX_d->text().toDouble();
    pidInfo.pid_i=ui->lineEdit_moveX_i->text().toDouble();
    pidInfo.pid_p=ui->lineEdit_moveX_p->text().toDouble();
    pidInfo.pid_velocity_p=0;
    mcInterpreter->setVehicleMagneticMovePid(1,pidInfo);
}

void MotionForm::on_pushButton_setVirtualPidMoveR_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改旋转控制pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    TrackPidInfo pidInfo;
    pidInfo.errorLimitForIntegration=ui->lineEdit_virtualLineR_errorLimitForIntegration->text().toDouble();
    pidInfo.feedforward=ui->lineEdit_virtualLineR_feedforward->text().toDouble();
    pidInfo.lineAdjustMax=0;
    pidInfo.rotationAdjustMax=0;
    pidInfo.pid_d=ui->lineEdit_moveR_d->text().toDouble();
    pidInfo.pid_i=ui->lineEdit_moveR_i->text().toDouble();
    pidInfo.pid_p=ui->lineEdit_moveR_p->text().toDouble();
    pidInfo.pid_velocity_p=0;
    mcInterpreter->setVehicleMagneticMovePid(2,pidInfo);

}

void MotionForm::on_pushButton_setVehicleAcceleration_clicked()
{
    mcInterpreter->setVehicleAcceleration(ui->lineEdit_vehicleAcceleration->text().toDouble());
}

void MotionForm::on_comboBox_activated(int index)
{

}

void MotionForm::on_pushButton_setVMarkParameter_clicked()
{
    VMarkDetectParameter vmarkParameterIn;
    vmarkParameterIn.rho_distanceAccuracy=ui->lineEdit_lineDetect_rho->text().toDouble();
    vmarkParameterIn.theta_angleAccuracy=ui->lineEdit_lineDetect_theta->text().toDouble();
    vmarkParameterIn.threshold_pt=ui->lineEdit_lineDetect_threshod->text().toDouble();
    vmarkParameterIn.minLineLength=ui->lineEdit_lineDetect_minLineLength->text().toDouble();
    vmarkParameterIn.maxLineGap=ui->lineEdit_lineDetect_maxLineGap->text().toDouble();

    vmarkParameterIn.vmark_min_angle=ui->lineEdit_vmark_minAngle->text().toDouble();
    vmarkParameterIn.vmark_max_angle=ui->lineEdit_vmark_maxAngle->text().toDouble();
    vmarkParameterIn.vmark_max_cross_distance=ui->lineEdit_vmark_maxDistance->text().toDouble();
    vmarkParameterIn.vmark_allow_vDistance=ui->lineEdit_vmark_distanceBias->text().toDouble();
    vmarkParameterIn.vmark_allow_vAngle=ui->lineEdit_vmark_angleBias->text().toDouble();

    mcInterpreter->setVmarkDetectParameter(vmarkParameterIn);

}

void MotionForm::on_pushButton_setBaseCompareMap_visionmatch_clicked()
{
    MapRegion baseRegionIn;
    baseRegionIn.originX=ui->lineEdit_baseOriginX_3->text().toDouble();
    baseRegionIn.originY=ui->lineEdit_baseOriginY_3->text().toDouble();
    baseRegionIn.height=ui->lineEdit_baseHeight_3->text().toDouble();
    baseRegionIn.width=ui->lineEdit_baseWidth_3->text().toDouble();
    mcInterpreter->setBaseMapRegion_visionMatch(baseRegionIn);
}

void MotionForm::on_pushButton_setLocalCompareMap_visionmatch_clicked()
{
//    MapRegion localRegionIn;
//    localRegionIn.originX=ui->lineEdit_baseOriginX_4->text().toDouble();
//    localRegionIn.originY=ui->lineEdit_baseOriginY_4->text().toDouble();
//    localRegionIn.height=ui->lineEdit_baseHeight_4->text().toDouble();
//    localRegionIn.width=ui->lineEdit_baseWidth_4->text().toDouble();
//    mcInterpreter->setLocalMapRegion_visionMatch(localRegionIn);
//    mcInterpreter->updateVMarkFile(ui->lineEdit_vmarkFIleName->text());
    mcInterpreter->setCurrentVMarkIndex(ui->lineEdit_vmark_index->text().toInt());
    mcInterpreter->setCurrentVMarkType(ui->lineEdit_structLineType->text().toInt());
    mcInterpreter->setLaserIntensityFilterByType_visionMatch(ui->lineEdit_laserFilterType->text().toInt());
    mcInterpreter->setCheckVehiclePosition_vmark(ui->checkBox_isCheckVmarkPosition->isChecked());
}

void MotionForm::on_pushButton_show_visionmatch_debugWindow_clicked()
{
    mcInterpreter->showDebugWindow_visionMatch(0);
}

void MotionForm::on_pushButton_calculate_vmark_laser_clicked()
{
    std::vector<double> x_out,y_out,r_out;
    int index=ui->lineEdit_vmark_index->text().toInt();
    mcInterpreter->detectVMark_laser(index,x_out,y_out,r_out);
    ui->lineEdit_visionmatch_vmark_size->setText(QString::number(x_out.size()));
    if(x_out.size()>0)
    {
        ui->lineEdit_vmark_x->setText(QString::number(x_out[0],'f',3));
        ui->lineEdit_vmark_y->setText(QString::number(y_out[0],'f',3));
        ui->lineEdit_vmark_r->setText(QString::number(180.0*r_out[0]/M_PI,'f',1));
    }
}

void MotionForm::on_pushButton_calculate_vmark_map_clicked()
{
    std::vector<double> x_out,y_out,r_out;
    mcInterpreter->detectVMark_baseMap(x_out,y_out,r_out);
    ui->lineEdit_visionmatch_vmark_size->setText(QString::number(x_out.size()));
    if(x_out.size()>0)
    {
        ui->lineEdit_vmark_x->setText(QString::number(x_out[0],'f',3));
        ui->lineEdit_vmark_y->setText(QString::number(y_out[0],'f',3));
        ui->lineEdit_vmark_r->setText(QString::number(r_out[0],'f',3));
    }
}

void MotionForm::on_checkBox_vmark_isWork_clicked(bool checked)
{
    mcInterpreter->setVmarkWork(checked);
}

void MotionForm::on_pushButton_show_laser_vmark_window_clicked()
{
    mcInterpreter->showDebugWindow_visionMatch(1);
}

void MotionForm::on_pushButton_get_amcl_position_clicked()
{
    E_VEHICLE_POSITION_TYPE typeIn=E_VEHICLE_POSITION_TYPE_ROS;
    PositionInfo positionReturn;
    mcInterpreter->getVehiclePosition_specified(positionReturn,typeIn);
    ui->lineEdit_baselink_pose_x->setText(QString::number(positionReturn.position.x,'f',3));
    ui->lineEdit_baselink_pose_y->setText(QString::number(positionReturn.position.y,'f',3));
    ui->lineEdit_baselink_pose_r->setText(QString::number(positionReturn.position.rotate,'f',3));
}

void MotionForm::on_pushButton_detect_vmark_in_laser_clicked()
{
    double baseLink_x=ui->lineEdit_baselink_pose_x->text().toDouble();
    double baseLink_y=ui->lineEdit_baselink_pose_y->text().toDouble();
    double baseLink_r=ui->lineEdit_baselink_pose_r->text().toDouble();
    vector<double> x_out;
    vector<double> y_out;
    vector<double> r_out;
    int vmarkType=ui->lineEdit_vmark_type->text().toInt();
    mcInterpreter->setCurrentVMarkType(vmarkType);
    mcInterpreter->calculateVMarkPosition_byLaser(baseLink_x,baseLink_y,baseLink_r,x_out,y_out,r_out);
    if(x_out.size()==1)
    {
        ui->lineEdit_vmark_pose_x->setText(QString::number(x_out[0]*1000,'f',3));
        ui->lineEdit_vmark_pose_y->setText(QString::number(y_out[0]*1000,'f',3));
        ui->lineEdit_vmark_pose_r->setText(QString::number(r_out[0]*180/M_PI,'f',3));
    }
    else
    {
        ui->lineEdit_vmark_pose_x->setText("failed");
        ui->lineEdit_vmark_pose_y->setText(QString::number(y_out.size()));
    }
    mcInterpreter->showDebugWindow_visionMatch(1);
}

void MotionForm::on_pushButton_saveVmarkFIle_clicked()
{
    VMarkInfo infoIn;
    infoIn.comment=ui->lineEdit_vmark_comment->text().toStdString();
    infoIn.mapName=ui->lineEdit_vmark_mapName->text().toStdString();
    infoIn.vindex=ui->lineEdit_vmark_index_save->text().toInt();
    infoIn.vmarkType=ui->lineEdit_vmark_type->text().toInt();
    infoIn.pose_x=ui->lineEdit_vmark_pose_x->text().toDouble();
    infoIn.pose_y=ui->lineEdit_vmark_pose_y->text().toDouble();
    infoIn.pose_r=ui->lineEdit_vmark_pose_r->text().toDouble();

    QString vmarkFileIn=ui->lineEdit_vmark_fileName->text();
    mcInterpreter->createVMark(infoIn,vmarkFileIn);
}

void MotionForm::on_pushButton_updateVmarkInfoByFile_clicked()
{
    mcInterpreter->updateVMarkFile(ui->lineEdit_vmark_fileName->text());
}

void MotionForm::on_pushButton_getVmakrsInfo_clicked()
{
    displayVmarksInfoList();
}

int MotionForm::updateVmarkPositionInworld()
{
    VehiclePosition positionOut;
    bool isSucess;
    mcInterpreter->getVMarkPositionInWorld(positionOut,isSucess);
    ui->lineEdit_vmark_pose_x->setText(QString::number(positionOut.x*1000,'f',3));
    ui->lineEdit_vmark_pose_y->setText(QString::number(positionOut.y*1000,'f',3));
    ui->lineEdit_vmark_pose_r->setText(QString::number(positionOut.rotate*180/M_PI,'f',3));
    if(true==isSucess)
    {
        ui->lineEdit_vmarkInWorldIsScuess->setText("sucess");
    }
    else
    {
        ui->lineEdit_vmarkInWorldIsScuess->setText("failed");
    }
    return 1;
}

void MotionForm::on_pushButton_setVirtualPid_lineBias_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改轨迹跟踪pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    double kp_errorDistance=ui->lineEdit_moveR_kp_errorDistance->text().toDouble();
    double kp_errorAngle=ui->lineEdit_moveR_kp_errorAngle->text().toDouble();
    double kp_errorOnlyRotation=ui->lineEdit_moveR_kp_errorOnlyRotation->text().toDouble();
    double kp_errorDistance_y=ui->lineEdit_moveY_kp_errorDistance->text().toDouble();
    mcInterpreter->setVirtualLinePid(kp_errorDistance,kp_errorAngle,kp_errorOnlyRotation,kp_errorDistance_y);
}

void MotionForm::on_pushButton_setVehicleAcceleration_rotate_clicked()
{
    mcInterpreter->setVehicleAcceleration_rotate(ui->lineEdit_vehicleAcceleration_rotate->text().toDouble());
}



void MotionForm::on_checkBox_isPrintVmark_clicked(bool checked)
{
    mcInterpreter->setIsPrintVmarkProcessData(checked);
}

void MotionForm::on_comboBox_2_currentIndexChanged(int index)
{
    mcInterpreter->setLineFittingMethod(index);
}

void MotionForm::on_pushButton_setVirtualPidMoveY_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改平移控制pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    TrackPidInfo pidInfo;
    pidInfo.errorLimitForIntegration=ui->lineEdit_virtualLineY_errorLimitForIntegration->text().toDouble();
    pidInfo.feedforward=ui->lineEdit_virtualLineY_feedforward->text().toDouble();
    pidInfo.lineAdjustMax=0;
    pidInfo.rotationAdjustMax=0;
    pidInfo.pid_d=ui->lineEdit_moveY_d->text().toDouble();
    pidInfo.pid_i=ui->lineEdit_moveY_i->text().toDouble();
    pidInfo.pid_p=ui->lineEdit_moveY_p->text().toDouble();
    pidInfo.pid_velocity_p=0;
    mcInterpreter->setVehicleMagneticMovePid(3,pidInfo);
}

void MotionForm::on_pushButton_setDebugFlag_clicked()
{
    int debug=ui->lineEdit_debugFlag->text().toInt();
    mcInterpreter->setDebugFlag(debug);
}

int MotionForm::showOdometerInfo()
{
    VehicleVelocity vehicleVelFeedbackOut;
    VehiclePosition vehiclePositionFeedbackOut;
    mcInterpreter->getOdometer(vehicleVelFeedbackOut,vehiclePositionFeedbackOut);
    ui->lineEdit_odometerX->setText(QString::number(vehiclePositionFeedbackOut.x,'f',3));
    ui->lineEdit_odometerY->setText(QString::number(vehiclePositionFeedbackOut.y,'f',3));
    ui->lineEdit_odometerR->setText(QString::number(180*vehiclePositionFeedbackOut.rotate/M_PI,'f',3));
    ui->lineEdit_odometerXv->setText(QString::number(vehicleVelFeedbackOut.x_move,'f',3));
    ui->lineEdit_odometerYv->setText(QString::number(vehicleVelFeedbackOut.y_move,'f',3));
    ui->lineEdit_odometerRv->setText(QString::number(180*vehicleVelFeedbackOut.z_rotate/M_PI,'f',3));
    return 1;
}

int MotionForm::showJointFeedback()
{
    QVector<double> position,vel,acc,torque;
    mcInterpreter->getJoint_Feedback(position,vel,acc,torque);
    jointNumer=position.size();
    ui->lineEdit_allJointNumber->setText(QString::number(jointNumer));
    if(jointNumer>=1)
    {
        ui->lineEdit_feedbackPositionJ1->setText(QString::number(position[0],'f',3));
        ui->lineEdit_feedbackVelJ1->setText(QString::number(vel[0],'f',2));
        ui->lineEdit_feedbackTorqueJ1->setText(QString::number(torque[0]));
    }
    if(jointNumer>=2)
    {
        ui->lineEdit_feedbackPositionJ2->setText(QString::number(position[1],'f',3));
        ui->lineEdit_feedbackVelJ2->setText(QString::number(vel[1],'f',2));
        ui->lineEdit_feedbackTorqueJ2->setText(QString::number(torque[1]));
    }
    if(jointNumer>=3)
    {
        ui->lineEdit_feedbackPositionJ3->setText(QString::number(position[2],'f',3));
        ui->lineEdit_feedbackVelJ3->setText(QString::number(vel[2],'f',2));
        ui->lineEdit_feedbackTorqueJ3->setText(QString::number(torque[2]));
    }
    if(jointNumer>=4)
    {
        ui->lineEdit_feedbackPositionJ4->setText(QString::number(position[3],'f',3));
        ui->lineEdit_feedbackVelJ4->setText(QString::number(vel[3],'f',2));
        ui->lineEdit_feedbackTorqueJ4->setText(QString::number(torque[3]));
    }
    if(jointNumer>=5)
    {
        ui->lineEdit_feedbackPositionJ5->setText(QString::number(position[4],'f',3));
        ui->lineEdit_feedbackVelJ5->setText(QString::number(vel[4],'f',2));
        ui->lineEdit_feedbackTorqueJ5->setText(QString::number(torque[4]));
    }
    if(jointNumer>=6)
    {
        ui->lineEdit_feedbackPositionJ6->setText(QString::number(position[5],'f',3));
        ui->lineEdit_feedbackVelJ6->setText(QString::number(vel[5],'f',2));
        ui->lineEdit_feedbackTorqueJ6->setText(QString::number(torque[5]));
    }
    if(jointNumer>=7)
    {
        ui->lineEdit_feedbackPositionJ7->setText(QString::number(position[6],'f',3));
        ui->lineEdit_feedbackVelJ7->setText(QString::number(vel[6],'f',2));
        ui->lineEdit_feedbackTorqueJ7->setText(QString::number(torque[6]));
    }
    if(jointNumer>=8)
    {
        ui->lineEdit_feedbackPositionJ8->setText(QString::number(position[7],'f',3));
        ui->lineEdit_feedbackVelJ8->setText(QString::number(vel[7],'f',2));
        ui->lineEdit_feedbackTorqueJ8->setText(QString::number(torque[7]));
    }


}

int MotionForm::showJointCommand()
{
    //read vel command, maganetic model
    if(2==ui->comboBox->currentIndex())
    {
        QVector<double> position_comand,vel_comand,acc_comand,torque_comand;
        mcInterpreter->getJointCommand(position_comand,vel_comand,acc_comand,torque_comand);
        if(1<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ1->setText(QString::number(position_comand[0]));
            ui->lineEdit_velCommandJ1->setText(QString::number(vel_comand[0],'f',2));
        }
        if(2<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ2->setText(QString::number(position_comand[1]));
            ui->lineEdit_velCommandJ2->setText(QString::number(vel_comand[1],'f',2));
        }
        if(3<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ3->setText(QString::number(position_comand[2]));
            ui->lineEdit_velCommandJ3->setText(QString::number(vel_comand[2],'f',2));
        }
        if(4<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ4->setText(QString::number(position_comand[3]));
            ui->lineEdit_velCommandJ4->setText(QString::number(vel_comand[3],'f',2));
        }
        if(5<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ5->setText(QString::number(position_comand[4]));
            ui->lineEdit_velCommandJ5->setText(QString::number(vel_comand[4],'f',2));
        }
        if(6<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ6->setText(QString::number(position_comand[5]));
            ui->lineEdit_velCommandJ6->setText(QString::number(vel_comand[5],'f',2));
        }
        if(7<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ7->setText(QString::number(position_comand[6]));
            ui->lineEdit_velCommandJ7->setText(QString::number(vel_comand[6],'f',2));
        }
        if(8<=position_comand.size())
        {
            ui->lineEdit_positionCommandJ8->setText(QString::number(position_comand[7]));
            ui->lineEdit_velCommandJ8->setText(QString::number(vel_comand[7],'f',2));
        }
    }

}

void MotionForm::on_pushButton_setQrcodeDebugFlag_clicked()
{
    mcInterpreter->showQrcodeDebugWindow(ui->comboBox_qrcodeDebugShowFlag->currentIndex());
}

void MotionForm::on_pushButton_setQrcodeDetectPara_clicked()
{
    QrcodeDetectParameter parameterIn;
    parameterIn.allowMinArea=ui->lineEdit_qrcodeMinAera->text().toDouble();
    parameterIn.allowMaxArea=ui->lineEdit_qrcodeMaxAera->text().toDouble();
    parameterIn.allowMinLength=ui->lineEdit_qrcodeMinLen->text().toDouble();
    parameterIn.allowMaxLength=ui->lineEdit_qrcodeMaxLen->text().toDouble();
    parameterIn.colorThrehold=ui->lineEdit_qrcodeColorThrehold->text().toDouble();
    mcInterpreter->setQrcodeDetectParameter(parameterIn);
    QMessageBox::information(NULL, "qrcode para", "参数设置成功", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setMotionFormDebugFlag_clicked()
{
    int tmpFlag=ui->lineEdit_motionFormDebugFlag->text().toInt();
    setDebugFlag(tmpFlag);
}

int MotionForm::showPositionType()
{
    E_VEHICLE_POSITION_TYPE typeOut;
    mcInterpreter->getVehiclePositionType(typeOut);
    ui->comboBox_currentPositionType->setCurrentIndex(typeOut);
    return 1;
}

int MotionForm::displayLaserDriverStatus()
{
    QVector<int> statusOut;
    mcInterpreter->getInternalLaserDriverStatus(statusOut);
    ui->lineEdit_internalLaserDriverNum->setText(QString::number(statusOut.size()));
    if(1<=statusOut.size())
    {
       ui->lineEdit_frontLaserDriverStatus->setText(QString::number(statusOut[0]));
    }
    if(2<=statusOut.size())
    {
        ui->lineEdit_backLaserDriverStatus->setText(QString::number(statusOut[1]));
    }
}

void MotionForm::on_pushButton_setRegulator_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改轴位置环控制pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    int axisId=ui->lineEdit_regulatorAxisId->text().toInt();
    double p=ui->lineEdit_regulator_p->text().toDouble();
    double i=ui->lineEdit_regulator_i->text().toDouble();
    double d=ui->lineEdit_regulator_d->text().toDouble();
    double errorLimitForIntegration=ui->lineEdit_regulator_errorLimitForIntegration->text().toDouble();
    double feedforward=ui->lineEdit_regulator_feedforward->text().toDouble();
    mcInterpreter->setPositionPid(axisId,p,i,d,errorLimitForIntegration,feedforward);
    QMessageBox::information(NULL, "change position pid", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setRegulatorVelocity_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改轴速度环控制pid参数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    int axisId=ui->lineEdit_regulatorAxisId_2->text().toInt();
    double p=ui->lineEdit_regulator_p_2->text().toDouble();
    double i=ui->lineEdit_regulator_i_2->text().toDouble();
    double d=ui->lineEdit_regulator_d_2->text().toDouble();
    double errorLimitForIntegration=ui->lineEdit_regulator_errorLimitForIntegration_2->text().toDouble();
    double feedforward=ui->lineEdit_regulator_feedforward_2->text().toDouble();
    mcInterpreter->setVelocityPid(axisId,p,i,d,errorLimitForIntegration,feedforward);
    QMessageBox::information(NULL, "change velocity pid", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_browser_wave_files_clicked()
{
    //定义文件对话框类
       QFileDialog *fileDialog = new QFileDialog(this);
       fileDialog->resize(400,500);
       //定义文件对话框标题
       fileDialog->setWindowTitle(tr("选择文件"));
       //设置默认文件路径
       QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
       QString configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/wave/";
       fileDialog->setDirectory(configFile);
       //设置文件过滤器
       fileDialog->setNameFilter(tr("*"));
       //设置可以选择多个文件,默认为只能选择一个文件QFileDialog::ExistingFiles
       fileDialog->setFileMode(QFileDialog::ExistingFiles);
       //设置视图模式
       fileDialog->setViewMode(QFileDialog::Detail);
       //打印所有选择的文件的路径
       QStringList fileNames;
       if(fileDialog->exec())
       {
           fileNames = fileDialog->selectedFiles();
       }
        qDebug()<<fileNames;
        if(fileNames.size()>0)
        {

            QString filePath =fileNames[0];
           int first = filePath.lastIndexOf ("/"); //从后面查找"/"位置
           QString title = filePath.right (filePath.length ()-first-1); //从右边截取
            ui->lineEdit_waveFileName->setText(title);
        }
}

void MotionForm::on_pushButton_open_wave_file_clicked()
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/wave/";
    configFile+=ui->lineEdit_waveFileName->text();
    motorView->openRecordFile(configFile);
    QMessageBox::information(NULL, "open", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_replot_clicked()
{
    motorView->plotFileWave();
    QMessageBox::information(NULL, "replot file", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_save_wave_file_clicked()
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString configFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/wave/";
    configFile+=ui->lineEdit_waveFileName->text();
    motorView->saveWaveDataToFile(configFile);
    QMessageBox::information(NULL, "save", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_replot_inner_clicked()
{
    motorView->plotInnerWave();
    QMessageBox::information(NULL, "replot inner", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_resetRosTimeMeasure_clicked()
{
    mcInterpreter->resetRosMeasureTime();
}

void MotionForm::on_pushButton_initialTrack_clicked()
{

     mcInterpreter->runVisionTrack(ui->comboBox_trackVisionIndex->currentIndex());
}

void MotionForm::on_pushButton_trigVisionOnce_clicked()
{
    mcInterpreter->trigVisionOnce(ui->comboBox_trackVisionIndex->currentIndex());
}

void MotionForm::on_pushButton_getTrackData_clicked()
{
    mcInterpreter->getTrackData(ui->comboBox_trackBeltIndex->currentIndex());
}

void MotionForm::on_pushButton_trackStart_clicked()
{
    mcInterpreter->beltTrackStart(ui->comboBox_trackBeltIndex->currentIndex());
}

void MotionForm::on_pushButton_trackEnd_clicked()
{
     mcInterpreter->beltTrackEnd(ui->comboBox_trackBeltIndex->currentIndex());
}

void MotionForm::on_pushButton_getVisionData_clicked()
{
    mcInterpreter->getVisionData(ui->comboBox_trackVisionIndex->currentIndex());
}

void MotionForm::on_pushButton_setDebugFlag_ros_clicked()
{
    int tmpFlag=ui->lineEdit_debugFlag_ros->text().toInt();
    mcInterpreter->setRosDebugFlag(tmpFlag);
}

void MotionForm::on_comboBox_laserFilterType_currentIndexChanged(int index)
{
    mcInterpreter->setOriginalLaserFilterType((E_LASER_FILTER_TYPE)index);
}

void MotionForm::on_pushButton_setInitialPose_clicked()
{
    int tmpFlag;
    VehiclePosition positionIn;
    positionIn.x=ui->lineEdit_initialPose_x->text().toDouble();
    positionIn.y=ui->lineEdit_initialPose_y->text().toDouble();
    positionIn.rotate=ui->lineEdit_initialPose_r->text().toDouble()*M_PI/180.0;
    mcInterpreter->setVehiclePosition(positionIn,1,tmpFlag);

}

void MotionForm::on_pushButton_setDebugFlag_reflectMatch_clicked()
{
    mcInterpreter->setReflectMatchDebugFlag(ui->lineEdit_debugFlag_reflectMatch->text().toInt());
}

void MotionForm::on_checkBox_jogAgvStart_clicked(bool checked)
{
    double x,y,r;
    if(checked)
    {
        QMessageBox message(QMessageBox::Question, "提示", "是否开始手动！"
                            , QMessageBox::Yes|QMessageBox::No , NULL);
        if(message.exec() == QMessageBox::No)
        {
            return;
        }
        x=ui->lineEdit_jogAgv_x->text().toInt()/100.0;
        y=ui->lineEdit_jogAgv_y->text().toInt()/100.0;
        r=ui->lineEdit_jogAgv_r->text().toInt()/100.0;
    }
    else
    {
        x=0;
        y=0;
        r=0;
    }
    mcInterpreter->vehicleJog(x,y,r);
}

void MotionForm::on_pushButton_setDebugFlag_vehicleJog_clicked()
{
    mcInterpreter->setVehicleJogDebugFlag(ui->lineEdit_debugFlag_vehicleJog->text().toInt());

}

void MotionForm::on_pushButton_setVirtualPid_steeringControl_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改转向轮控制比例系数！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    mcInterpreter->setSteeringControlKp(ui->lineEdit_steeringControlKp->text().toDouble()
                                        ,ui->lineEdit_steeringAdjustMax->text().toDouble());
}

void MotionForm::on_pushButton_setVmarkKalmanFilter_clicked()
{
    int id=ui->lineEdit_vmark_kalmanFilter_id->text().toInt();
    double q=ui->lineEdit_vmark_kalmanFilter_Q->text().toDouble();
    double r=ui->lineEdit_vmark_kalmanFilter_R->text().toDouble();
    mcInterpreter->setVisionMatchKalmanFilterPara(id,q,r);

}

void MotionForm::on_pushButton_setEncoderOutput_clicked()
{
    mcInterpreter->setEncoderReceiverOutput(ui->lineEdit_visionTrackReceive_output->text().toInt());
}

void MotionForm::on_pushButton_setBeltTrackPara_clicked()
{
    QMessageBox message(QMessageBox::Question, "提示", "是否修改跟踪位置补偿值！"
                        , QMessageBox::Yes|QMessageBox::No , NULL);
    if(message.exec() == QMessageBox::No)
    {
        return;
    }
    mcInterpreter->setTrackOffset(ui->lineEdit_beltTrack_index->text().toInt(),
               ui->lineEdit_beltTrack_offsetX->text().toDouble(),ui->lineEdit_beltTrack_offsetY->text().toDouble());

}

void MotionForm::on_pushButton_setWeldWave_clicked()
{
    E_WAVE_TYPE typeIn=(E_WAVE_TYPE)ui->comboBox_waveMotionType->currentIndex();
    double waveWidthIn=ui->lineEdit_waveMotionWidth->text().toDouble();
    double wavePeriodIn=ui->lineEdit_waveMotionPeriod->text().toDouble();
    mcInterpreter->setWaveType(typeIn,waveWidthIn,wavePeriodIn);
    QMessageBox::information(NULL, "设置摆弧", "sucess", QMessageBox::Yes );

}


void MotionForm::on_pushButton_setWeldWaveStart_clicked()
{
    mcInterpreter->startWave();
    QMessageBox::information(NULL, "startWave", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setWeldWaveEnd_clicked()
{
    mcInterpreter->stopWave();
    QMessageBox::information(NULL, "stopWave", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setLineTrajMaxVelocity_clicked()
{
    mcInterpreter->setRobotArmLineMaxVelocity(ui->lineEdit_lineMaxVelocity->text().toDouble());
    QMessageBox::information(NULL, "设置line速度", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setLineTrajMaxAcceleration_clicked()
{
    mcInterpreter->setRobotArmLineMaxAcceleration(ui->lineEdit_lineMaxAcceleration->text().toDouble());
    QMessageBox::information(NULL, "设置line加速度", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setJointTrajMaxVelocity_clicked()
{
     mcInterpreter->setRobotArmJointMaxVelocity(ui->lineEdit_logicAxisIdForSet->text().toInt()
                 ,ui->lineEdit_jointMaxVelocity->text().toDouble());
     QMessageBox::information(NULL, "设置axis速度", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setJointTrajMaxAcceleration_clicked()
{
    mcInterpreter->setRobotArmJointMaxAcceleration(ui->lineEdit_logicAxisIdForSet->text().toInt()
                                                   ,ui->lineEdit_jointMaxAcceleration->text().toDouble());
     QMessageBox::information(NULL, "设置axis加速度", "sucess", QMessageBox::Yes );

}

void MotionForm::on_pushButton_setVelStyle_clicked()
{
    mcInterpreter->setVelocityStyle((E_VELOCITY_TYPE)ui->comboBox_velocityStyle->currentIndex());
     QMessageBox::information(NULL, "设置vel style", "sucess", QMessageBox::Yes );
}



void MotionForm::on_pushButton_setRoatate_equiradius_clicked()
{
    mcInterpreter->setRotateEqradius(ui->lineEdit_rotate_equirdius->text().toDouble());
     QMessageBox::information(NULL, "设置equirdius", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setDragPara_clicked()
{
    DragPara paraIn;
    paraIn.controlKp=ui->lineEdit_dragKp->text().toDouble();
    paraIn.thresholdTorque=ui->lineEdit_dragThresholdTorque->text().toDouble();
    paraIn.maxPositionError=ui->lineEdit_dragMaxErrorP->text().toDouble();
    paraIn.filterCount=ui->lineEdit_dragFilterCount->text().toInt();
    int axis=ui->lineEdit_dragAxisId->text().toInt();

    mcInterpreter->setDragPara(axis,paraIn);

    QMessageBox::information(NULL, "设置drag", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setSegment_clicked()
{
    JointAttribute paraIn;
    paraIn.mass=ui->lineEdit_segmentMass->text().toDouble();
    paraIn.massCenter.resize(3);
    paraIn.massCenter[0]=ui->lineEdit_segmentMassX->text().toDouble();
    paraIn.massCenter[1]=ui->lineEdit_segmentMassY->text().toDouble();
    paraIn.massCenter[2]=ui->lineEdit_segmentMassZ->text().toDouble();
    int axis=ui->lineEdit_segmentAxisId->text().toInt();

    mcInterpreter->setSegmentAttribute(axis,paraIn);

    QMessageBox::information(NULL, "设置segment", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setCollisionDetectRatio_clicked()
{

    double ratio=ui->lineEdit_setCollisionDetectRatio->text().toDouble();
    mcInterpreter->setCollisionDetectRatio(ratio);
    QMessageBox::information(NULL, "设置collisionRatio", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setCollisionTorqueBias_clicked()
{
    int axis=ui->lineEdit_collisionAxisid->text().toInt();
    double torque=ui->lineEdit_collisionTorqueBias->text().toDouble();
    mcInterpreter->setCollisionWarnTorque(axis,torque);
    QMessageBox::information(NULL, "设置collision torque", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setEmcCarvingTp_clicked()
{
    mcInterpreter->setIsInCarvingMode(true);
    QMessageBox::information(NULL, "切换为新插补ok", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setTraditionalTp_clicked()
{
    mcInterpreter->setIsInCarvingMode(false);
    QMessageBox::information(NULL, "切换为旧插补ok", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setTpNetFeedScale_clicked()
{
    mcInterpreter->setTpNetFeedScale(ui->lineEdit_tpNetFeedScale->text().toDouble());
    QMessageBox::information(NULL, "set", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_algroCallTimeReset_clicked()
{
    mcInterpreter->resetAlgroCallTimeMeasure();
}

void MotionForm::on_pushButton_addCircle_clicked()
{
    int idIn=ui->lineEdit_circle_id->text().toInt();
    std::vector<double> posIn;
    posIn.push_back(ui->lineEdit_circle_pos_x->text().toDouble());
    posIn.push_back(ui->lineEdit_circle_pos_y->text().toDouble());
    posIn.push_back(ui->lineEdit_circle_pos_z->text().toDouble());
    posIn.push_back(ui->lineEdit_circle_pos_w->text().toDouble());
    posIn.push_back(ui->lineEdit_circle_pos_p->text().toDouble());
    posIn.push_back(ui->lineEdit_circle_pos_r->text().toDouble());
    posIn.push_back(0);
    posIn.push_back(0);
    posIn.push_back(0);
    std::vector<double> centerIn;
    centerIn.push_back(ui->lineEdit_circle_center_x->text().toDouble());
    centerIn.push_back(ui->lineEdit_circle_center_y->text().toDouble());
    centerIn.push_back(ui->lineEdit_circle_center_z->text().toDouble());
    std::vector<double> normalIn;
    normalIn.push_back(ui->lineEdit_circle_normal_x->text().toDouble());
    normalIn.push_back(ui->lineEdit_circle_normal_y->text().toDouble());
    normalIn.push_back(ui->lineEdit_circle_normal_z->text().toDouble());
    double vel=ui->lineEdit_circle_vel->text().toDouble();
//    double acc=ui->lineEdit_circle_acc->text().toDouble();
    int turns=ui->lineEdit_circle_turns->text().toInt();
    if(1==mcInterpreter->addCircle(idIn,posIn,centerIn,normalIn,vel,turns,ENUM_MMPS))
    {
        QMessageBox::information(NULL, "set", "add circle sucess", QMessageBox::Yes );
    }
    else
    {
        QMessageBox::information(NULL, "set", "add circle failed", QMessageBox::Yes );
    }

}

void MotionForm::on_pushButton_setTpMode_clicked()
{
    mcInterpreter->setAxisPlanMode(ui->lineEdit_axisId->text().toInt(),(E_AXIS_PLAN_MODE)ui->comboBox_tpMode->currentIndex());
    QMessageBox::information(NULL, "set", "sucess", QMessageBox::Yes );
}

void MotionForm::on_checkBox_simpleEnable_clicked(bool checked)
{
    mcInterpreter->controlAxisSimpleTp(ui->lineEdit_axisId->text().toInt(),checked);
    QMessageBox::information(NULL, "set", "sucess", QMessageBox::Yes );
}

void MotionForm::on_pushButton_setTpTarget_clicked()
{
    mcInterpreter->setAxisSimpleTpTarget(ui->lineEdit_axisId->text().toInt()
                                         ,ui->lineEdit_simpletpTarget->text().toDouble()
                                          ,ui->lineEdit_simpletpVel->text().toDouble()
                                          ,ui->lineEdit_simpletpAcc->text().toDouble());
    QMessageBox::information(NULL, "set", "sucess", QMessageBox::Yes );
}
